Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 04:13:39 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

fix stabilization reference quaternion for normal rotorcrafts

Browse Source
This commit is contained in:
Felix Ruess
2012-03-03 17:08:59 +01:00
parent 6e1072abea
commit 18cbd1ac64
4 changed files with 73 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/autopilot/subsystems/rotorcraft/stabilization_int_quat.makefile
+5
View File
@@ -0,0 +1,5 @@
ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT
ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/stabilization_attitude_int.h\"
ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/stabilization_attitude_ref_quat_int.h\"
ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_ref_quat_int.c
ap.srcs += $(SRC_FIRMWARE)/stabilization/stabilization_attitude_quat_int.c
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+22 -14
View File
@@ -96,7 +96,7 @@ void stabilization_attitude_init(void) {
stabilization_attitude_ref_init();
/*
#if 0
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]);
@@ -108,31 +108,35 @@ void stabilization_attitude_init(void) {
VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
}
*/
#endif
INT32_QUAT_ZERO( stabilization_att_sum_err_quat );
INT_EULERS_ZERO( stabilization_att_sum_err );
}
/*
#if 0
void stabilization_attitude_gain_schedule(uint8_t idx)
{
if (gain_idx >= STABILIZATION_ATTITUDE_GAIN_NB) {
// This could be bad -- Just say no.
return;
}
gain_idx = idx;
stabilization_attitude_ref_schedule(idx);
if (gain_idx >= STABILIZATION_ATTITUDE_GAIN_NB) {
// This could be bad -- Just say no.
return;
}
gain_idx = idx;
stabilization_attitude_ref_schedule(idx);
}
*/
#endif
void stabilization_attitude_enter(void) {
#if !USE_SETPOINTS_WITH_TRANSITIONS
/* reset psi setpoint to current psi angle */
stab_att_sp_euler.psi = ahrs.ltp_to_body_euler.psi;
#endif
stabilization_attitude_ref_enter();
INT32_QUAT_ZERO( stabilization_att_sum_err_quat );
//FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers );
INT_EULERS_ZERO( stabilization_att_sum_err );
INT32_QUAT_ZERO(stabilization_att_sum_err_quat);
INT_EULERS_ZERO(stabilization_att_sum_err);
}
#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
@@ -220,6 +224,10 @@ void stabilization_attitude_run(bool_t enable_integrator) {
void stabilization_attitude_read_rc(bool_t in_flight) {
STABILIZATION_ATTITUDE_READ_RC(stab_att_sp_euler, in_flight);
#if USE_SETPOINTS_WITH_TRANSITIONS
stabilization_attitude_read_rc_absolute(in_flight);
#else
stabilization_attitude_read_rc_setpoint(in_flight);
#endif
}
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
+10 -27
View File
@@ -22,7 +22,7 @@
*/
/** \file stabilization_attitude_ref_int.c
* \brief Booz attitude reference generation (quaternion int version)
* \brief rotorcraft attitude reference generation (quaternion int version)
*
*/
@@ -62,9 +62,9 @@
#define OMEGA_2_R BFP_OF_REAL((OMEGA_R*OMEGA_R), OMEGA_2_R_RES)
struct Int32Eulers stab_att_sp_euler;
struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC
struct Int32Quat stab_att_sp_quat;
struct Int32Eulers stab_att_ref_euler;
struct Int32Eulers stab_att_ref_euler; ///< with #INT32_ANGLE_FRAC
struct Int32Quat stab_att_ref_quat;
struct Int32Rates stab_att_ref_rate;
struct Int32Rates stab_att_ref_accel;
@@ -89,26 +89,14 @@ static void reset_psi_ref_from_body(void) {
stab_att_ref_accel.r = 0;
}
static void update_ref_quat_from_eulers(void) {
struct Int32RMat ref_rmat;
#ifdef STICKS_RMAT312
INT32_RMAT_OF_EULERS_312(ref_rmat, stab_att_ref_euler);
#else
INT32_RMAT_OF_EULERS_321(ref_rmat, stab_att_ref_euler);
#endif
INT32_QUAT_OF_RMAT(stab_att_ref_quat, ref_rmat);
INT32_QUAT_WRAP_SHORTEST(stab_att_ref_quat);
}
void stabilization_attitude_ref_init(void) {
INT_EULERS_ZERO(stab_att_sp_euler);
INT32_QUAT_ZERO( stab_att_sp_quat);
INT32_QUAT_ZERO(stab_att_sp_quat);
INT_EULERS_ZERO(stab_att_ref_euler);
INT32_QUAT_ZERO( stab_att_ref_quat);
INT_RATES_ZERO( stab_att_ref_rate);
INT_RATES_ZERO( stab_att_ref_accel);
INT32_QUAT_ZERO(stab_att_ref_quat);
INT_RATES_ZERO(stab_att_ref_rate);
INT_RATES_ZERO(stab_att_ref_accel);
/*
for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
@@ -122,23 +110,18 @@ void stabilization_attitude_ref_init(void) {
void stabilization_attitude_ref_enter()
{
reset_psi_ref_from_body();
stabilization_attitude_sp_enter();
memcpy(&stab_att_ref_quat, &stab_att_sp_quat, sizeof(struct Int32Quat));
update_quat_from_eulers(&stab_att_ref_quat, &stab_att_ref_euler);
//memcpy(&stab_att_ref_quat, &stab_att_sp_quat, sizeof(struct Int32Quat));
memset(&stab_att_ref_accel, 0, sizeof(struct Int32Rates));
memset(&stab_att_ref_rate, 0, sizeof(struct Int32Rates));
//update_ref_quat_from_eulers();
}
/*
* Reference
*/
#define DT_UPDATE (1./512.)
#define DT_UPDATE (1./PERIODIC_FREQUENCY)
#define F_UPDATE_RES 9
#include "messages.h"
#include "mcu_periph/uart.h"
#include "subsystems/datalink/downlink.h"
void stabilization_attitude_ref_update() {
/* integrate reference attitude */
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h
+36 -18
View File
@@ -22,26 +22,15 @@
#define STABILIZATION_ATTITUDE_INT_REF_QUAT_INT_H
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/quat_setpoint_int.h"
//#include "firmwares/rotorcraft/stabilization/quat_setpoint_int.h"
#include "subsystems/radio_control.h"
#include "math/pprz_algebra_float.h"
#include "stabilization_attitude_ref_int.h"
#define REF_ACCEL_FRAC 12
#define REF_RATE_FRAC 16
#define REF_ANGLE_FRAC 20
#include "subsystems/ahrs.h"
#define REF_ANGLE_PI BFP_OF_REAL(3.1415926535897932384626433832795029, REF_ANGLE_FRAC)
#define REF_ANGLE_TWO_PI BFP_OF_REAL(2.*3.1415926535897932384626433832795029, REF_ANGLE_FRAC)
#define ANGLE_REF_NORMALIZE(_a) { \
while (_a > REF_ANGLE_PI) _a -= REF_ANGLE_TWO_PI; \
while (_a < -REF_ANGLE_PI) _a += REF_ANGLE_TWO_PI; \
}
#define RC_UPDATE_FREQ 40.
#define ROLL_COEF (STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ)
// FIXME: unused, what was it supposed to be?
//#define ROLL_COEF_H (STABILIZATION_ATTITUDE_SP_MAX_P_H / MAX_PPRZ)
@@ -62,12 +51,41 @@
#define PITCH_DEADBAND_EXCEEDED() \
(radio_control.values[RADIO_PITCH] > STABILIZATION_ATTITUDE_DEADBAND_E || \
radio_control.values[RADIO_PITCH] < -STABILIZATION_ATTITUDE_DEADBAND_E)
#define YAW_DEADBAND_EXCEEDED() \
(radio_control.values[RADIO_YAW] > STABILIZATION_ATTITUDE_DEADBAND_R || \
radio_control.values[RADIO_YAW] < -STABILIZATION_ATTITUDE_DEADBAND_R)
#define STABILIZATION_ATTITUDE_READ_RC(_sp, _in_flight) do { stabilization_attitude_read_rc_absolute(_sp, _in_flight); } while(0)
#define STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp) do {} while(0)
#define STABILIZATION_ATTITUDE_RESET_PSI_REF(_sp) {}
static inline void update_quat_from_eulers(struct Int32Quat *quat, struct Int32Eulers *eulers) {
struct Int32RMat rmat;
#ifdef STICKS_RMAT312
INT32_RMAT_OF_EULERS_312(rmat, *eulers);
#else
INT32_RMAT_OF_EULERS_321(rmat, *eulers);
#endif
INT32_QUAT_OF_RMAT(*quat, rmat);
INT32_QUAT_WRAP_SHORTEST(*quat);
}
static inline void stabilization_attitude_read_rc_setpoint(bool_t in_flight) {
stab_att_sp_euler.phi = ((int32_t)-radio_control.values[RADIO_ROLL] * SP_MAX_PHI / MAX_PPRZ);
stab_att_sp_euler.theta = ((int32_t) radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ);
if (in_flight) {
if (YAW_DEADBAND_EXCEEDED()) {
stab_att_sp_euler.psi += ((int32_t)-radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ);
INT32_ANGLE_NORMALIZE(stab_att_sp_euler.psi);
}
}
else { /* if not flying, use current yaw as setpoint */
stab_att_sp_euler.psi = ahrs.ltp_to_body_euler.psi;
}
/* update quaternion setpoint from euler setpoint */
update_quat_from_eulers(&stab_att_sp_quat, &stab_att_sp_euler);
}
void stabilization_attitude_ref_enter(void);
void stabilization_attitude_ref_schedule(uint8_t idx);