Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-01 04:46:51 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

changed rotorcraft stabilization_attitude_quat gains to positive

Browse Source
This commit is contained in:
Felix Ruess
2011-11-30 23:10:29 +01:00
parent 3167c8fecc
commit c8aa382e87
3 changed files with 66 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
+17 -17
View File
@@ -86,15 +86,15 @@ void stabilization_attitude_init(void) {
stabilization_attitude_ref_init();
for (int i = 0; i < STABILIZATION_ATTITUDE_FLOAT_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]);
VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]);
VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
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]);
VECT3_ASSIGN_ABS(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
}
FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat );
@@ -136,35 +136,35 @@ static void attitude_run_fb(float fb_commands[], struct FloatAttitudeGains *gain
{
/* PID feedback */
fb_commands[COMMAND_ROLL] =
GAIN_PRESCALER_P * -gains->p.x * att_err->qx +
GAIN_PRESCALER_P * gains->p.x * att_err->qx +
GAIN_PRESCALER_D * gains->d.x * rate_err->p +
GAIN_PRESCALER_D * gains->rates_d.x * rate_err_d->p +
GAIN_PRESCALER_I * gains->i.x * sum_err->qx;
fb_commands[COMMAND_PITCH] =
GAIN_PRESCALER_P * -gains->p.y * att_err->qy +
GAIN_PRESCALER_P * gains->p.y * att_err->qy +
GAIN_PRESCALER_D * gains->d.y * rate_err->q +
GAIN_PRESCALER_D * gains->rates_d.y * rate_err_d->q +
GAIN_PRESCALER_I * gains->i.y * sum_err->qy;
fb_commands[COMMAND_YAW] =
GAIN_PRESCALER_P * -gains->p.z * att_err->qz +
GAIN_PRESCALER_P * gains->p.z * att_err->qz +
GAIN_PRESCALER_D * gains->d.z * rate_err->r +
GAIN_PRESCALER_D * gains->rates_d.z * rate_err_d->r +
GAIN_PRESCALER_I * gains->i.z * sum_err->qz;
fb_commands[COMMAND_ROLL_SURFACE] =
GAIN_PRESCALER_P * -gains->surface_p.x * att_err->qx +
GAIN_PRESCALER_P * gains->surface_p.x * att_err->qx +
GAIN_PRESCALER_D * gains->surface_d.x * rate_err->p +
GAIN_PRESCALER_I * gains->surface_i.x * sum_err->qx;
fb_commands[COMMAND_PITCH_SURFACE] =
GAIN_PRESCALER_P * -gains->surface_p.y * att_err->qy +
GAIN_PRESCALER_P * gains->surface_p.y * att_err->qy +
GAIN_PRESCALER_D * gains->surface_d.y * rate_err->q +
GAIN_PRESCALER_I * gains->surface_i.y * sum_err->qy;
fb_commands[COMMAND_YAW_SURFACE] =
GAIN_PRESCALER_P * -gains->surface_p.z * att_err->qz +
GAIN_PRESCALER_P * gains->surface_p.z * att_err->qz +
GAIN_PRESCALER_D * gains->surface_d.z * rate_err->r +
GAIN_PRESCALER_I * gains->surface_i.z * sum_err->qz;
@@ -189,7 +189,7 @@ void stabilization_attitude_run(bool_t enable_integrator) {
/* rate error */
struct FloatRates rate_err;
RATES_DIFF(rate_err, ahrs_float.body_rate, stab_att_ref_rate);
RATES_DIFF(rate_err, stab_att_ref_rate, ahrs_float.body_rate);
/* integrated error */
if (enable_integrator) {
@@ -199,7 +199,7 @@ void stabilization_attitude_run(bool_t enable_integrator) {
scaled_att_err.qx = att_err.qx / IERROR_SCALE;
scaled_att_err.qy = att_err.qy / IERROR_SCALE;
scaled_att_err.qz = att_err.qz / IERROR_SCALE;
FLOAT_QUAT_COMP_INV(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
FLOAT_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
FLOAT_QUAT_NORMALIZE(new_sum_err);
FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat);
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+35 -20
View File
@@ -33,12 +33,7 @@
#include "subsystems/ahrs.h"
#include "generated/airframe.h"
struct Int32AttitudeGains stabilization_gains = {
{STABILIZATION_ATTITUDE_PHI_PGAIN, STABILIZATION_ATTITUDE_THETA_PGAIN, STABILIZATION_ATTITUDE_PSI_PGAIN },
{STABILIZATION_ATTITUDE_PHI_DGAIN, STABILIZATION_ATTITUDE_THETA_DGAIN, STABILIZATION_ATTITUDE_PSI_DGAIN },
{STABILIZATION_ATTITUDE_PHI_DDGAIN, STABILIZATION_ATTITUDE_THETA_DDGAIN, STABILIZATION_ATTITUDE_PSI_DDGAIN },
{STABILIZATION_ATTITUDE_PHI_IGAIN, STABILIZATION_ATTITUDE_THETA_IGAIN, STABILIZATION_ATTITUDE_PSI_IGAIN }
};
struct Int32AttitudeGains stabilization_gains;
struct Int32Quat stabilization_att_sum_err_quat;
struct Int32Eulers stabilization_att_sum_err;
@@ -98,18 +93,38 @@ void stabilization_attitude_init(void) {
/*
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]);
VECT3_ASSIGN(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]);
VECT3_ASSIGN(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
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]);
VECT3_ASSIGN_ABS(stabilization_gains[i].p, phi_pgain[i], theta_pgain[i], psi_pgain[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].d, phi_dgain[i], theta_dgain[i], psi_dgain[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].i, phi_igain[i], theta_igain[i], psi_igain[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].dd, phi_ddgain[i], theta_ddgain[i], psi_ddgain[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].rates_d, phi_dgain_d[i], theta_dgain_d[i], psi_dgain_d[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]);
VECT3_ASSIGN_ABS(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
}
*/
VECT3_ASSIGN_ABS(stabilization_gains.p,
STABILIZATION_ATTITUDE_PHI_PGAIN,
STABILIZATION_ATTITUDE_THETA_PGAIN,
STABILIZATION_ATTITUDE_PSI_PGAIN);
VECT3_ASSIGN_ABS(stabilization_gains.d,
STABILIZATION_ATTITUDE_PHI_DGAIN,
STABILIZATION_ATTITUDE_THETA_DGAIN,
STABILIZATION_ATTITUDE_PSI_DGAIN);
VECT3_ASSIGN_ABS(stabilization_gains.i,
STABILIZATION_ATTITUDE_PHI_IGAIN,
STABILIZATION_ATTITUDE_THETA_IGAIN,
STABILIZATION_ATTITUDE_PSI_IGAIN);
VECT3_ASSIGN_ABS(stabilization_gains.dd,
STABILIZATION_ATTITUDE_PHI_DDGAIN,
STABILIZATION_ATTITUDE_THETA_DDGAIN,
STABILIZATION_ATTITUDE_PSI_DDGAIN);
INT32_QUAT_ZERO( stabilization_att_sum_err_quat );
INT_EULERS_ZERO( stabilization_att_sum_err );
}
@@ -152,17 +167,17 @@ static void attitude_run_fb(int32_t fb_commands[], struct Int32AttitudeGains *ga
{
/* PID feedback */
fb_commands[COMMAND_ROLL] =
GAIN_PRESCALER_P * -gains->p.x * QUAT1_FLOAT_OF_BFP(att_err->qx) / 4 +
GAIN_PRESCALER_P * gains->p.x * QUAT1_FLOAT_OF_BFP(att_err->qx) / 4 +
GAIN_PRESCALER_D * gains->d.x * RATE_FLOAT_OF_BFP(rate_err->p) / 16 +
GAIN_PRESCALER_I * gains->i.x * QUAT1_FLOAT_OF_BFP(sum_err->qx) / 2;
fb_commands[COMMAND_PITCH] =
GAIN_PRESCALER_P * -gains->p.y * QUAT1_FLOAT_OF_BFP(att_err->qy) / 4 +
GAIN_PRESCALER_P * gains->p.y * QUAT1_FLOAT_OF_BFP(att_err->qy) / 4 +
GAIN_PRESCALER_D * gains->d.y * RATE_FLOAT_OF_BFP(rate_err->q) / 16 +
GAIN_PRESCALER_I * gains->i.y * QUAT1_FLOAT_OF_BFP(sum_err->qy) / 2;
fb_commands[COMMAND_YAW] =
GAIN_PRESCALER_P * -gains->p.z * QUAT1_FLOAT_OF_BFP(att_err->qz) / 4 +
GAIN_PRESCALER_P * gains->p.z * QUAT1_FLOAT_OF_BFP(att_err->qz) / 4 +
GAIN_PRESCALER_D * gains->d.z * RATE_FLOAT_OF_BFP(rate_err->r) / 16 +
GAIN_PRESCALER_I * gains->i.z * QUAT1_FLOAT_OF_BFP(sum_err->qz) / 2;
@@ -188,7 +203,7 @@ void stabilization_attitude_run(bool_t enable_integrator) {
/* rate error */
struct Int32Rates rate_err;
RATES_DIFF(rate_err, ahrs.body_rate, stab_att_ref_rate);
RATES_DIFF(rate_err, stab_att_ref_rate, ahrs.body_rate);
/* integrated error */
if (enable_integrator) {
@@ -198,7 +213,7 @@ void stabilization_attitude_run(bool_t enable_integrator) {
scaled_att_err.qx = att_err.qx / IERROR_SCALE;
scaled_att_err.qy = att_err.qy / IERROR_SCALE;
scaled_att_err.qz = att_err.qz / IERROR_SCALE;
INT32_QUAT_COMP_INV(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
INT32_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err);
INT32_QUAT_NORMALIZE(new_sum_err);
QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err);
INT32_EULERS_OF_QUAT(stabilization_att_sum_err, stabilization_att_sum_err_quat);
sw/airborne/math/pprz_algebra.h
+14
View File
@@ -27,6 +27,7 @@
#include <float.h> /* for FLT_EPSILON */
#include <string.h> /* for memcpy */
#include "std.h" /* for ABS */
#define SQUARE(_a) ((_a)*(_a))
@@ -101,6 +102,13 @@
(_a).z = (_z); \
}
/* a = {abs(x), abs(y), abs(z)} */
#define VECT3_ASSIGN_ABS(_a, _x, _y, _z) { \
(_a).x = ABS(_x); \
(_a).y = ABS(_y); \
(_a).z = ABS(_z); \
}
/* a = b */
#define VECT3_COPY(_a, _b) { \
(_a).x = (_b).x; \
@@ -199,6 +207,12 @@
if ((_v).z > (_v_max).y) (_v).z = (_v_max).z; else if ((_v).z < (_v_min).z) (_v).z = (_v_min).z; \
}
/* */
#define VECT3_ABS(_vo, _vi) { \
(_vo).x = ABS((_vi).x); \
(_vo).y = ABS((_vi).y); \
(_vo).z = ABS((_vi).z); \
}
/*