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 27 Packages Projects Releases Wiki Activity

cleanup naming in horizontal guidance reference, rename b2_gh to guidance_h

Browse Source
This commit is contained in:
Felix Ruess
2010-11-24 15:07:04 +01:00
parent 0ce81c491a
commit 77de7baf02
2 changed files with 87 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+5 -5
View File
@@ -71,7 +71,7 @@ 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); \
guidance_h_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); \
@@ -194,7 +194,7 @@ void guidance_h_run(bool_t in_flight) {
else {
INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot);
#ifdef GUIDANCE_H_USE_REF
b2_gh_update_ref_from_pos_sp(guidance_h_pos_sp);
guidance_h_update_ref_from_pos_sp(guidance_h_pos_sp);
#endif
#ifndef STABILISATION_ATTITUDE_TYPE_FLOAT
guidance_h_psi_sp = (nav_heading << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC));
@@ -283,9 +283,9 @@ __attribute__ ((always_inline)) static inline void guidance_h_nav_run(bool_t in
/* convert our reference to generic representation */
#ifdef GUIDANCE_H_USE_REF
INT32_VECT2_RSHIFT(guidance_h_pos_ref, b2_gh_pos_ref, (B2_GH_POS_REF_FRAC - INT32_POS_FRAC));
INT32_VECT2_LSHIFT(guidance_h_speed_ref, b2_gh_speed_ref, (INT32_SPEED_FRAC - B2_GH_SPEED_REF_FRAC));
INT32_VECT2_LSHIFT(guidance_h_accel_ref, b2_gh_accel_ref, (INT32_ACCEL_FRAC - B2_GH_ACCEL_REF_FRAC));
INT32_VECT2_RSHIFT(guidance_h_pos_ref, guidance_h_pos_ref, (GUIDANCE_H_POS_REF_FRAC - INT32_POS_FRAC));
INT32_VECT2_LSHIFT(guidance_h_speed_ref, guidance_h_speed_ref, (INT32_SPEED_FRAC - GUIDANCE_H_SPEED_REF_FRAC));
INT32_VECT2_LSHIFT(guidance_h_accel_ref, guidance_h_accel_ref, (INT32_ACCEL_FRAC - GUIDANCE_H_ACCEL_REF_FRAC));
#else
VECT2_COPY(guidance_h_pos_ref, guidance_h_pos_sp);
INT_VECT2_ZERO(guidance_h_speed_ref);
sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h
+82 -82
View File
@@ -30,35 +30,35 @@
#include "math/pprz_algebra_int.h"
/* update frequency */
#define B2_GH_FREQ_FRAC 9
#define B2_GH_FREQ (1<<B2_GH_FREQ_FRAC)
#define GUIDANCE_H_FREQ_FRAC 9
#define GUIDANCE_H_FREQ (1<<GUIDANCE_H_FREQ_FRAC)
/* reference model accel in meters/sec2 (output) */
/* Q23.8 : accuracy 0.0039 , range 8388km/s2 */
/* int32_4_8_t */
extern struct Int32Vect2 b2_gh_accel_ref;
#define B2_GH_ACCEL_REF_FRAC 8
extern struct Int32Vect2 guidance_h_accel_ref;
#define GUIDANCE_H_ACCEL_REF_FRAC 8
/* reference model speed in meters/sec (output) */
/* Q14.17 : accuracy 0.0000076 , range 16384m/s2 */
extern struct Int32Vect2 b2_gh_speed_ref;
#define B2_GH_SPEED_REF_FRAC (B2_GH_ACCEL_REF_FRAC + B2_GH_FREQ_FRAC)
extern struct Int32Vect2 guidance_h_speed_ref;
#define GUIDANCE_H_SPEED_REF_FRAC (GUIDANCE_H_ACCEL_REF_FRAC + GUIDANCE_H_FREQ_FRAC)
/* reference model position in meters (output) */
/* Q37.26 : */
extern struct Int64Vect2 b2_gh_pos_ref;
#define B2_GH_POS_REF_FRAC (B2_GH_SPEED_REF_FRAC + B2_GH_FREQ_FRAC)
extern struct Int64Vect2 guidance_h_pos_ref;
#define GUIDANCE_H_POS_REF_FRAC (GUIDANCE_H_SPEED_REF_FRAC + GUIDANCE_H_FREQ_FRAC)
/* Saturations definition */
#ifndef GUIDANCE_H_REF_MAX_ACCEL
#define GUIDANCE_H_REF_MAX_ACCEL ( tanf(RadOfDeg(30.))*9.81 )
#endif
#define B2_GH_MAX_ACCEL BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, B2_GH_ACCEL_REF_FRAC)
#define GUIDANCE_H_MAX_ACCEL BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GUIDANCE_H_ACCEL_REF_FRAC)
#ifndef GUIDANCE_H_REF_MAX_SPEED
#define GUIDANCE_H_REF_MAX_SPEED ( 5. )
#endif
#define B2_GH_MAX_SPEED BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, B2_GH_SPEED_REF_FRAC)
#define GUIDANCE_H_MAX_SPEED BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, GUIDANCE_H_SPEED_REF_FRAC)
/* second order model natural frequency and damping */
#ifndef GUIDANCE_H_REF_OMEGA
@@ -67,122 +67,122 @@ extern struct Int64Vect2 b2_gh_pos_ref;
#ifndef GUIDANCE_H_REF_ZETA
#define GUIDANCE_H_REF_ZETA 0.85
#endif
#define B2_GH_ZETA_OMEGA_FRAC 10
#define B2_GH_ZETA_OMEGA BFP_OF_REAL((GUIDANCE_H_REF_ZETA*GUIDANCE_H_REF_OMEGA), B2_GH_ZETA_OMEGA_FRAC)
#define B2_GH_OMEGA_2_FRAC 7
#define B2_GH_OMEGA_2 BFP_OF_REAL((GUIDANCE_H_REF_OMEGA*GUIDANCE_H_REF_OMEGA), B2_GH_OMEGA_2_FRAC)
#define GUIDANCE_H_ZETA_OMEGA_FRAC 10
#define GUIDANCE_H_ZETA_OMEGA BFP_OF_REAL((GUIDANCE_H_REF_ZETA*GUIDANCE_H_REF_OMEGA), GUIDANCE_H_ZETA_OMEGA_FRAC)
#define GUIDANCE_H_OMEGA_2_FRAC 7
#define GUIDANCE_H_OMEGA_2 BFP_OF_REAL((GUIDANCE_H_REF_OMEGA*GUIDANCE_H_REF_OMEGA), GUIDANCE_H_OMEGA_2_FRAC)
/* first order time constant */
#define B2_GH_REF_THAU_F 0.5
#define B2_GH_REF_INV_THAU_FRAC 16
#define B2_GH_REF_INV_THAU BFP_OF_REAL((1./B2_GH_REF_THAU_F), B2_GH_REF_INV_THAU_FRAC)
#define GUIDANCE_H_REF_THAU_F 0.5
#define GUIDANCE_H_REF_INV_THAU_FRAC 16
#define GUIDANCE_H_REF_INV_THAU BFP_OF_REAL((1./GUIDANCE_H_REF_THAU_F), GUIDANCE_H_REF_INV_THAU_FRAC)
#ifdef GUIDANCE_H_C
static inline void b2_gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel);
static inline void b2_gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp);
static inline void b2_gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp);
static inline void guidance_h_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel);
static inline void guidance_h_update_ref_from_pos_sp(struct Int32Vect2 pos_sp);
static inline void guidance_h_update_ref_from_speed_sp(struct Int32Vect2 speed_sp);
struct Int64Vect2 b2_gh_pos_ref;
struct Int32Vect2 b2_gh_speed_ref;
struct Int32Vect2 b2_gh_accel_ref;
struct Int64Vect2 guidance_h_pos_ref;
struct Int32Vect2 guidance_h_speed_ref;
struct Int32Vect2 guidance_h_accel_ref;
static inline void b2_gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel) {
static inline void guidance_h_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel) {
struct Int64Vect2 new_pos;
new_pos.x = ((int64_t)pos.x)<<(B2_GH_POS_REF_FRAC - INT32_POS_FRAC);
new_pos.y = ((int64_t)pos.y)<<(B2_GH_POS_REF_FRAC - INT32_POS_FRAC);
b2_gh_pos_ref = new_pos;
INT32_VECT2_RSHIFT(b2_gh_speed_ref, speed, (INT32_SPEED_FRAC - B2_GH_SPEED_REF_FRAC));
INT32_VECT2_RSHIFT(b2_gh_accel_ref, accel, (INT32_ACCEL_FRAC - B2_GH_ACCEL_REF_FRAC));
new_pos.x = ((int64_t)pos.x)<<(GUIDANCE_H_POS_REF_FRAC - INT32_POS_FRAC);
new_pos.y = ((int64_t)pos.y)<<(GUIDANCE_H_POS_REF_FRAC - INT32_POS_FRAC);
guidance_h_pos_ref = new_pos;
INT32_VECT2_RSHIFT(guidance_h_speed_ref, speed, (INT32_SPEED_FRAC - GUIDANCE_H_SPEED_REF_FRAC));
INT32_VECT2_RSHIFT(guidance_h_accel_ref, accel, (INT32_ACCEL_FRAC - GUIDANCE_H_ACCEL_REF_FRAC));
}
static inline void b2_gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) {
static inline void guidance_h_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) {
VECT2_ADD(b2_gh_pos_ref, b2_gh_speed_ref);
VECT2_ADD(b2_gh_speed_ref, b2_gh_accel_ref);
VECT2_ADD(guidance_h_pos_ref, guidance_h_speed_ref);
VECT2_ADD(guidance_h_speed_ref, guidance_h_accel_ref);
// compute the "speed part" of accel = -2*zeta*omega*speed -omega^2(pos - pos_sp)
struct Int32Vect2 speed;
INT32_VECT2_RSHIFT(speed, b2_gh_speed_ref, (B2_GH_SPEED_REF_FRAC - B2_GH_ACCEL_REF_FRAC));
VECT2_SMUL(speed, speed, -2*B2_GH_ZETA_OMEGA);
INT32_VECT2_RSHIFT(speed, speed, B2_GH_ZETA_OMEGA_FRAC);
INT32_VECT2_RSHIFT(speed, guidance_h_speed_ref, (GUIDANCE_H_SPEED_REF_FRAC - GUIDANCE_H_ACCEL_REF_FRAC));
VECT2_SMUL(speed, speed, -2*GUIDANCE_H_ZETA_OMEGA);
INT32_VECT2_RSHIFT(speed, speed, GUIDANCE_H_ZETA_OMEGA_FRAC);
// compute pos error in pos_sp resolution
struct Int32Vect2 pos_err;
INT32_VECT2_RSHIFT(pos_err, b2_gh_pos_ref, (B2_GH_POS_REF_FRAC - INT32_POS_FRAC));
INT32_VECT2_RSHIFT(pos_err, guidance_h_pos_ref, (GUIDANCE_H_POS_REF_FRAC - INT32_POS_FRAC));
VECT2_DIFF(pos_err, pos_err, pos_sp);
// convert to accel resolution
INT32_VECT2_RSHIFT(pos_err, pos_err, (INT32_POS_FRAC - B2_GH_ACCEL_REF_FRAC));
INT32_VECT2_RSHIFT(pos_err, pos_err, (INT32_POS_FRAC - GUIDANCE_H_ACCEL_REF_FRAC));
// compute the "pos part" of accel
struct Int32Vect2 pos;
VECT2_SMUL(pos, pos_err, (-B2_GH_OMEGA_2));
INT32_VECT2_RSHIFT(pos, pos, B2_GH_OMEGA_2_FRAC);
VECT2_SMUL(pos, pos_err, (-GUIDANCE_H_OMEGA_2));
INT32_VECT2_RSHIFT(pos, pos, GUIDANCE_H_OMEGA_2_FRAC);
// sum accel
VECT2_SUM(b2_gh_accel_ref, speed, pos);
VECT2_SUM(guidance_h_accel_ref, speed, pos);
/* Saturate accelerations */
VECT2_STRIM(b2_gh_accel_ref, -B2_GH_MAX_ACCEL, B2_GH_MAX_ACCEL);
VECT2_STRIM(guidance_h_accel_ref, -GUIDANCE_H_MAX_ACCEL, GUIDANCE_H_MAX_ACCEL);
/* Saturate speed and adjust acceleration accordingly */
if (b2_gh_speed_ref.x <= -B2_GH_MAX_SPEED) {
b2_gh_speed_ref.x = -B2_GH_MAX_SPEED;
if (b2_gh_accel_ref.x < 0)
b2_gh_accel_ref.x = 0;
if (guidance_h_speed_ref.x <= -GUIDANCE_H_MAX_SPEED) {
guidance_h_speed_ref.x = -GUIDANCE_H_MAX_SPEED;
if (guidance_h_accel_ref.x < 0)
guidance_h_accel_ref.x = 0;
}
else if (b2_gh_speed_ref.x >= B2_GH_MAX_SPEED) {
b2_gh_speed_ref.x = B2_GH_MAX_SPEED;
if (b2_gh_accel_ref.x > 0)
b2_gh_accel_ref.x = 0;
else if (guidance_h_speed_ref.x >= GUIDANCE_H_MAX_SPEED) {
guidance_h_speed_ref.x = GUIDANCE_H_MAX_SPEED;
if (guidance_h_accel_ref.x > 0)
guidance_h_accel_ref.x = 0;
}
if (b2_gh_speed_ref.y <= -B2_GH_MAX_SPEED) {
b2_gh_speed_ref.y = -B2_GH_MAX_SPEED;
if (b2_gh_accel_ref.y < 0)
b2_gh_accel_ref.y = 0;
if (guidance_h_speed_ref.y <= -GUIDANCE_H_MAX_SPEED) {
guidance_h_speed_ref.y = -GUIDANCE_H_MAX_SPEED;
if (guidance_h_accel_ref.y < 0)
guidance_h_accel_ref.y = 0;
}
else if (b2_gh_speed_ref.y >= B2_GH_MAX_SPEED) {
b2_gh_speed_ref.y = B2_GH_MAX_SPEED;
if (b2_gh_accel_ref.y > 0)
b2_gh_accel_ref.y = 0;
else if (guidance_h_speed_ref.y >= GUIDANCE_H_MAX_SPEED) {
guidance_h_speed_ref.y = GUIDANCE_H_MAX_SPEED;
if (guidance_h_accel_ref.y > 0)
guidance_h_accel_ref.y = 0;
}
}
static inline void b2_gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) {
static inline void guidance_h_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) {
VECT2_ADD(b2_gh_pos_ref, b2_gh_speed_ref);
VECT2_ADD(b2_gh_speed_ref, b2_gh_accel_ref);
VECT2_ADD(guidance_h_pos_ref, guidance_h_speed_ref);
VECT2_ADD(guidance_h_speed_ref, guidance_h_accel_ref);
// compute speed error
struct Int32Vect2 speed_err;
INT32_VECT2_RSHIFT(speed_err, speed_sp, (INT32_SPEED_FRAC - B2_GH_SPEED_REF_FRAC));
VECT2_DIFF(speed_err, b2_gh_speed_ref, speed_err);
INT32_VECT2_RSHIFT(speed_err, speed_sp, (INT32_SPEED_FRAC - GUIDANCE_H_SPEED_REF_FRAC));
VECT2_DIFF(speed_err, guidance_h_speed_ref, speed_err);
// convert to accel resolution
INT32_VECT2_RSHIFT(speed_err, speed_err, (B2_GH_SPEED_REF_FRAC - B2_GH_ACCEL_REF_FRAC));
INT32_VECT2_RSHIFT(speed_err, speed_err, (GUIDANCE_H_SPEED_REF_FRAC - GUIDANCE_H_ACCEL_REF_FRAC));
// compute accel from speed_sp
VECT2_SMUL(b2_gh_accel_ref, speed_err, -B2_GH_REF_INV_THAU);
INT32_VECT2_RSHIFT(b2_gh_accel_ref, b2_gh_accel_ref, B2_GH_REF_INV_THAU_FRAC);
VECT2_SMUL(guidance_h_accel_ref, speed_err, -GUIDANCE_H_REF_INV_THAU);
INT32_VECT2_RSHIFT(guidance_h_accel_ref, guidance_h_accel_ref, GUIDANCE_H_REF_INV_THAU_FRAC);
/* Saturate accelerations */
VECT2_STRIM(b2_gh_accel_ref, -B2_GH_MAX_ACCEL, B2_GH_MAX_ACCEL);
VECT2_STRIM(guidance_h_accel_ref, -GUIDANCE_H_MAX_ACCEL, GUIDANCE_H_MAX_ACCEL);
/* Saturate speed and adjust acceleration accordingly */
if (b2_gh_speed_ref.x <= -B2_GH_MAX_SPEED) {
b2_gh_speed_ref.x = -B2_GH_MAX_SPEED;
if (b2_gh_accel_ref.x < 0)
b2_gh_accel_ref.x = 0;
if (guidance_h_speed_ref.x <= -GUIDANCE_H_MAX_SPEED) {
guidance_h_speed_ref.x = -GUIDANCE_H_MAX_SPEED;
if (guidance_h_accel_ref.x < 0)
guidance_h_accel_ref.x = 0;
}
else if (b2_gh_speed_ref.x >= B2_GH_MAX_SPEED) {
b2_gh_speed_ref.x = B2_GH_MAX_SPEED;
if (b2_gh_accel_ref.x > 0)
b2_gh_accel_ref.x = 0;
else if (guidance_h_speed_ref.x >= GUIDANCE_H_MAX_SPEED) {
guidance_h_speed_ref.x = GUIDANCE_H_MAX_SPEED;
if (guidance_h_accel_ref.x > 0)
guidance_h_accel_ref.x = 0;
}
if (b2_gh_speed_ref.y <= -B2_GH_MAX_SPEED) {
b2_gh_speed_ref.y = -B2_GH_MAX_SPEED;
if (b2_gh_accel_ref.y < 0)
b2_gh_accel_ref.y = 0;
if (guidance_h_speed_ref.y <= -GUIDANCE_H_MAX_SPEED) {
guidance_h_speed_ref.y = -GUIDANCE_H_MAX_SPEED;
if (guidance_h_accel_ref.y < 0)
guidance_h_accel_ref.y = 0;
}
else if (b2_gh_speed_ref.y >= B2_GH_MAX_SPEED) {
b2_gh_speed_ref.y = B2_GH_MAX_SPEED;
if (b2_gh_accel_ref.y > 0)
b2_gh_accel_ref.y = 0;
else if (guidance_h_speed_ref.y >= GUIDANCE_H_MAX_SPEED) {
guidance_h_speed_ref.y = GUIDANCE_H_MAX_SPEED;
if (guidance_h_accel_ref.y > 0)
guidance_h_accel_ref.y = 0;
}
}