Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 00:53:41 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[guidance_h_ref] turn some defines into static const

Browse Source
This commit is contained in:
Felix Ruess
2013-09-01 16:19:49 +02:00
parent e2961a419a
commit 34c72c0154
1 changed files with 16 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c
+16 -16
View File
@@ -53,7 +53,7 @@ struct Int64Vect2 gh_pos_ref;
#ifndef GUIDANCE_H_REF_MAX_ACCEL
#define GUIDANCE_H_REF_MAX_ACCEL 5.66
#endif
#define GH_MAX_ACCEL BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACCEL_REF_FRAC)
static const int32_t gh_max_accel = BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACCEL_REF_FRAC);
/** Speed saturation */
#ifndef GUIDANCE_H_REF_MAX_SPEED
@@ -61,7 +61,7 @@ struct Int64Vect2 gh_pos_ref;
#endif
/** @todo GH_MAX_SPEED must be limited to 2^14 to avoid overflow */
#define GH_MAX_SPEED_REF_FRAC 7
#define GH_MAX_SPEED BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, GH_MAX_SPEED_REF_FRAC)
static const int32_t gh_max_speed = BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, GH_MAX_SPEED_REF_FRAC);
/** second order model natural frequency */
#ifndef GUIDANCE_H_REF_OMEGA
@@ -72,16 +72,16 @@ struct Int64Vect2 gh_pos_ref;
#define GUIDANCE_H_REF_ZETA 0.85
#endif
#define GH_ZETA_OMEGA_FRAC 10
#define GH_ZETA_OMEGA BFP_OF_REAL((GUIDANCE_H_REF_ZETA*GUIDANCE_H_REF_OMEGA), GH_ZETA_OMEGA_FRAC)
#define GH_OMEGA_2_FRAC 7
#define GH_OMEGA_2 BFP_OF_REAL((GUIDANCE_H_REF_OMEGA*GUIDANCE_H_REF_OMEGA), GH_OMEGA_2_FRAC)
static const int32_t gh_zeta_omega = BFP_OF_REAL((GUIDANCE_H_REF_ZETA*GUIDANCE_H_REF_OMEGA), GH_ZETA_OMEGA_FRAC);
static const int32_t gh_omega_2= BFP_OF_REAL((GUIDANCE_H_REF_OMEGA*GUIDANCE_H_REF_OMEGA), GH_OMEGA_2_FRAC);
/** first order time constant */
#ifndef GUIDANCE_H_REF_TAU
#define GUIDANCE_H_REF_TAU 0.5
#endif
#define GH_REF_INV_TAU_FRAC 16
#define GH_REF_INV_TAU BFP_OF_REAL((1./GUIDANCE_H_REF_TAU), GH_REF_INV_TAU_FRAC)
static const int32_t gh_ref_inv_tau = BFP_OF_REAL((1./GUIDANCE_H_REF_TAU), GH_REF_INV_TAU_FRAC);
static struct Int32Vect2 gh_max_speed_ref;
static struct Int32Vect2 gh_max_accel_ref;
@@ -111,7 +111,7 @@ void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) {
// compute the "speed part" of accel = -2*zeta*omega*speed -omega^2(pos - pos_sp)
struct Int32Vect2 speed;
INT32_VECT2_RSHIFT(speed, gh_speed_ref, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC));
VECT2_SMUL(speed, speed, -2*GH_ZETA_OMEGA);
VECT2_SMUL(speed, speed, -2 * gh_zeta_omega);
INT32_VECT2_RSHIFT(speed, speed, GH_ZETA_OMEGA_FRAC);
// compute pos error in pos_sp resolution
struct Int32Vect2 pos_err;
@@ -121,7 +121,7 @@ void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) {
INT32_VECT2_RSHIFT(pos_err, pos_err, (INT32_POS_FRAC - GH_ACCEL_REF_FRAC));
// compute the "pos part" of accel
struct Int32Vect2 pos;
VECT2_SMUL(pos, pos_err, (-GH_OMEGA_2));
VECT2_SMUL(pos, pos_err, -gh_omega_2);
INT32_VECT2_RSHIFT(pos, pos, GH_OMEGA_2_FRAC);
// sum accel
VECT2_SUM(gh_accel_ref, speed, pos);
@@ -146,7 +146,7 @@ void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) {
// convert to accel resolution
INT32_VECT2_RSHIFT(speed_err, speed_err, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC));
// compute accel from speed_sp
VECT2_SMUL(gh_accel_ref, speed_err, -GH_REF_INV_TAU);
VECT2_SMUL(gh_accel_ref, speed_err, -gh_ref_inv_tau);
INT32_VECT2_RSHIFT(gh_accel_ref, gh_accel_ref, GH_REF_INV_TAU_FRAC);
/* Compute max ref accel/speed along route before saturation */
@@ -159,10 +159,10 @@ void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) {
static void gh_compute_max_ref(struct Int32Vect2* ref_vector) {
/* Compute route reference before saturation */
if (ref_vector->x == 0 && ref_vector->y == 0) {
gh_max_accel_ref.x = GH_MAX_ACCEL;
gh_max_accel_ref.y = GH_MAX_ACCEL;
gh_max_speed_ref.x = GH_MAX_SPEED;
gh_max_speed_ref.y = GH_MAX_SPEED;
gh_max_accel_ref.x = gh_max_accel;
gh_max_accel_ref.y = gh_max_accel;
gh_max_speed_ref.x = gh_max_speed;
gh_max_speed_ref.y = gh_max_speed;
}
else {
float f_route_ref = atan2f(-ref_vector->y, -ref_vector->x);
@@ -173,11 +173,11 @@ static void gh_compute_max_ref(struct Int32Vect2* ref_vector) {
c_route_ref = abs(c_route_ref);
s_route_ref = abs(s_route_ref);
/* Compute maximum acceleration*/
gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC);
gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC);
gh_max_accel_ref.x = INT_MULT_RSHIFT(gh_max_accel, c_route_ref, INT32_TRIG_FRAC);
gh_max_accel_ref.y = INT_MULT_RSHIFT(gh_max_accel, s_route_ref, INT32_TRIG_FRAC);
/* Compute maximum speed*/
gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC);
gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC);
gh_max_speed_ref.x = INT_MULT_RSHIFT(gh_max_speed, c_route_ref, INT32_TRIG_FRAC);
gh_max_speed_ref.y = INT_MULT_RSHIFT(gh_max_speed, s_route_ref, INT32_TRIG_FRAC);
/* restore gh_speed_ref range (Q14.17) */
INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC));
}