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

[rotorcraft] guidance_h_ref: cleanup comments, indentation, trailing whitespaces

Browse Source
This commit is contained in:
Felix Ruess
2013-03-26 11:15:56 +01:00
parent 29d855ed36
commit c0da3885e1
1 changed files with 58 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h
+58 -47
View File
@@ -1,5 +1,5 @@
/* /*
* Copyright (C) 2008-2009 ENAC <poinix@gmail.com> * Copyright (C) 2008-2013 The Paparazzi Team
* *
* This file is part of paparazzi. * This file is part of paparazzi.
* *
@@ -19,7 +19,7 @@
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*/ */
/** @file firmware/rotorcraft/guidance/guidance_h_ref.h /** @file firmwares/rotorcraft/guidance/guidance_h_ref.h
* Reference generation for horizontal guidance. * Reference generation for horizontal guidance.
* *
*/ */
@@ -36,41 +36,50 @@
#define B2_GH_FREQ_FRAC 9 #define B2_GH_FREQ_FRAC 9
#define B2_GH_FREQ (1<<B2_GH_FREQ_FRAC) #define B2_GH_FREQ (1<<B2_GH_FREQ_FRAC)
/* reference model accel in meters/sec2 (output) */ /** Reference model acceleration.
/* Q23.8 : accuracy 0.0039 , range 8388km/s2 */ * in meters/sec2 (output)
/* int32_4_8_t */ * fixed point representation: Q23.8
* accuracy 0.0039, range 8388km/s2
*/
extern struct Int32Vect2 b2_gh_accel_ref; extern struct Int32Vect2 b2_gh_accel_ref;
#define B2_GH_ACCEL_REF_FRAC 8 #define B2_GH_ACCEL_REF_FRAC 8
/* reference model speed in meters/sec (output) */ /** Reference model speed.
/* Q14.17 : accuracy 0.0000076 , range 16384m/s */ * in meters/sec
* with fixedpoint representation: Q14.17
* accuracy 0.0000076 , range 16384m/s
*/
extern struct Int32Vect2 b2_gh_speed_ref; extern struct Int32Vect2 b2_gh_speed_ref;
#define B2_GH_SPEED_REF_FRAC (B2_GH_ACCEL_REF_FRAC + B2_GH_FREQ_FRAC) #define B2_GH_SPEED_REF_FRAC (B2_GH_ACCEL_REF_FRAC + B2_GH_FREQ_FRAC)
/* reference model position in meters (output) */ /* Reference model position.
/* Q37.26 : */ * in meters
* with fixedpoint representation: Q37.26
*/
extern struct Int64Vect2 b2_gh_pos_ref; extern struct Int64Vect2 b2_gh_pos_ref;
#define B2_GH_POS_REF_FRAC (B2_GH_SPEED_REF_FRAC + B2_GH_FREQ_FRAC) #define B2_GH_POS_REF_FRAC (B2_GH_SPEED_REF_FRAC + B2_GH_FREQ_FRAC)
/* Accel saturation */ /** Accel saturation.
/* tanf(RadOfDeg(30.))*9.81 = 5.66 */ * tanf(RadOfDeg(30.))*9.81 = 5.66
*/
#ifndef GUIDANCE_H_REF_MAX_ACCEL #ifndef GUIDANCE_H_REF_MAX_ACCEL
#define GUIDANCE_H_REF_MAX_ACCEL 5.66 #define GUIDANCE_H_REF_MAX_ACCEL 5.66
#endif #endif
#define B2_GH_MAX_ACCEL BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, B2_GH_ACCEL_REF_FRAC) #define B2_GH_MAX_ACCEL BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, B2_GH_ACCEL_REF_FRAC)
/*Speed saturation*/ /** Speed saturation */
#ifndef GUIDANCE_H_REF_MAX_SPEED #ifndef GUIDANCE_H_REF_MAX_SPEED
#define GUIDANCE_H_REF_MAX_SPEED 5. #define GUIDANCE_H_REF_MAX_SPEED 5.
#endif #endif
/*FIX ME :B2_GH_MAX_SPEED must be limited to 2^14 to avoid overflow*/ /** @todo B2_GH_MAX_SPEED must be limited to 2^14 to avoid overflow */
#define B2_GH_MAX_SPEED_REF_FRAC 7 #define B2_GH_MAX_SPEED_REF_FRAC 7
#define B2_GH_MAX_SPEED BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, B2_GH_MAX_SPEED_REF_FRAC) #define B2_GH_MAX_SPEED BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, B2_GH_MAX_SPEED_REF_FRAC)
/* second order model natural frequency and damping */ /** second order model natural frequency */
#ifndef GUIDANCE_H_REF_OMEGA #ifndef GUIDANCE_H_REF_OMEGA
#define GUIDANCE_H_REF_OMEGA RadOfDeg(67.) #define GUIDANCE_H_REF_OMEGA RadOfDeg(67.)
#endif #endif
/** second order model damping */
#ifndef GUIDANCE_H_REF_ZETA #ifndef GUIDANCE_H_REF_ZETA
#define GUIDANCE_H_REF_ZETA 0.85 #define GUIDANCE_H_REF_ZETA 0.85
#endif #endif
@@ -79,7 +88,7 @@ extern struct Int64Vect2 b2_gh_pos_ref;
#define B2_GH_OMEGA_2_FRAC 7 #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 B2_GH_OMEGA_2 BFP_OF_REAL((GUIDANCE_H_REF_OMEGA*GUIDANCE_H_REF_OMEGA), B2_GH_OMEGA_2_FRAC)
/* first order time constant */ /** first order time constant */
#define B2_GH_REF_THAU_F 0.5 #define B2_GH_REF_THAU_F 0.5
#define B2_GH_REF_INV_THAU_FRAC 16 #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 B2_GH_REF_INV_THAU BFP_OF_REAL((1./B2_GH_REF_THAU_F), B2_GH_REF_INV_THAU_FRAC)
@@ -91,12 +100,14 @@ static inline void b2_gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp);
struct Int64Vect2 b2_gh_pos_ref; struct Int64Vect2 b2_gh_pos_ref;
struct Int32Vect2 b2_gh_speed_ref; struct Int32Vect2 b2_gh_speed_ref;
struct Int32Vect2 b2_gh_max_speed_ref;
struct Int32Vect2 b2_gh_accel_ref; struct Int32Vect2 b2_gh_accel_ref;
struct Int32Vect2 b2_gh_max_accel_ref;
int32_t route_ref; static struct Int32Vect2 b2_gh_max_speed_ref;
int32_t s_route_ref, c_route_ref; static struct Int32Vect2 b2_gh_max_accel_ref;
static int32_t route_ref;
static int32_t s_route_ref;
static int32_t c_route_ref;
static inline void b2_gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel) { static inline void b2_gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel) {
struct Int64Vect2 new_pos; struct Int64Vect2 new_pos;
@@ -129,23 +140,23 @@ static inline void b2_gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) {
INT32_VECT2_RSHIFT(pos, pos, B2_GH_OMEGA_2_FRAC); INT32_VECT2_RSHIFT(pos, pos, B2_GH_OMEGA_2_FRAC);
// sum accel // sum accel
VECT2_SUM(b2_gh_accel_ref, speed, pos); VECT2_SUM(b2_gh_accel_ref, speed, pos);
/* Compute route reference before saturation */ /* Compute route reference before saturation */
// use metric precision or values are too large // use metric precision or values are too large
INT32_ATAN2(route_ref,-pos_err.y,-pos_err.x); INT32_ATAN2(route_ref, -pos_err.y, -pos_err.x);
/* Compute North and East route components */ /* Compute North and East route components */
PPRZ_ITRIG_SIN(s_route_ref, route_ref); PPRZ_ITRIG_SIN(s_route_ref, route_ref);
PPRZ_ITRIG_COS(c_route_ref, route_ref); PPRZ_ITRIG_COS(c_route_ref, route_ref);
c_route_ref=abs(c_route_ref); c_route_ref = abs(c_route_ref);
s_route_ref=abs(s_route_ref); s_route_ref = abs(s_route_ref);
/* Compute maximum acceleration*/ /* Compute maximum acceleration*/
b2_gh_max_accel_ref.x= INT_MULT_RSHIFT((int32_t)B2_GH_MAX_ACCEL,c_route_ref,INT32_TRIG_FRAC); b2_gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)B2_GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC);
b2_gh_max_accel_ref.y= INT_MULT_RSHIFT((int32_t)B2_GH_MAX_ACCEL,s_route_ref,INT32_TRIG_FRAC); b2_gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)B2_GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC);
/* Compute maximum speed*/ /* Compute maximum speed*/
b2_gh_max_speed_ref.x= INT_MULT_RSHIFT((int32_t)B2_GH_MAX_SPEED,c_route_ref,INT32_TRIG_FRAC); b2_gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)B2_GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC);
b2_gh_max_speed_ref.y= INT_MULT_RSHIFT((int32_t)B2_GH_MAX_SPEED,s_route_ref,INT32_TRIG_FRAC); b2_gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)B2_GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC);
/* restore b2_gh_speed_ref range (Q14.17) */ /* restore b2_gh_speed_ref range (Q14.17) */
INT32_VECT2_LSHIFT(b2_gh_max_speed_ref, b2_gh_max_speed_ref,B2_GH_SPEED_REF_FRAC - B2_GH_MAX_SPEED_REF_FRAC); INT32_VECT2_LSHIFT(b2_gh_max_speed_ref, b2_gh_max_speed_ref, (B2_GH_SPEED_REF_FRAC - B2_GH_MAX_SPEED_REF_FRAC));
/* Saturate accelerations */ /* Saturate accelerations */
if (b2_gh_accel_ref.x <= -b2_gh_max_accel_ref.x) { if (b2_gh_accel_ref.x <= -b2_gh_max_accel_ref.x) {
@@ -160,7 +171,7 @@ static inline void b2_gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) {
else if (b2_gh_accel_ref.y >= b2_gh_max_accel_ref.y) { else if (b2_gh_accel_ref.y >= b2_gh_max_accel_ref.y) {
b2_gh_accel_ref.y = b2_gh_max_accel_ref.y; b2_gh_accel_ref.y = b2_gh_max_accel_ref.y;
} }
/* Saturate speed and adjust acceleration accordingly */ /* Saturate speed and adjust acceleration accordingly */
if (b2_gh_speed_ref.x <= -b2_gh_max_speed_ref.x) { if (b2_gh_speed_ref.x <= -b2_gh_max_speed_ref.x) {
b2_gh_speed_ref.x = -b2_gh_max_speed_ref.x; b2_gh_speed_ref.x = -b2_gh_max_speed_ref.x;
@@ -201,22 +212,22 @@ static inline void b2_gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) {
/* Compute route reference before saturation */ /* Compute route reference before saturation */
// use metric precision or values are too large // use metric precision or values are too large
INT32_ATAN2(route_ref,-speed_sp.y,-speed_sp.x); INT32_ATAN2(route_ref, -speed_sp.y, -speed_sp.x);
/* Compute North and East route components */ /* Compute North and East route components */
PPRZ_ITRIG_SIN(s_route_ref, route_ref); PPRZ_ITRIG_SIN(s_route_ref, route_ref);
PPRZ_ITRIG_COS(c_route_ref, route_ref); PPRZ_ITRIG_COS(c_route_ref, route_ref);
c_route_ref=abs(c_route_ref); c_route_ref = abs(c_route_ref);
s_route_ref=abs(s_route_ref); s_route_ref = abs(s_route_ref);
/* Compute maximum acceleration*/ /* Compute maximum acceleration*/
b2_gh_max_accel_ref.x= INT_MULT_RSHIFT((int32_t)B2_GH_MAX_ACCEL,c_route_ref,INT32_TRIG_FRAC); b2_gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)B2_GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC);
b2_gh_max_accel_ref.y= INT_MULT_RSHIFT((int32_t)B2_GH_MAX_ACCEL,s_route_ref,INT32_TRIG_FRAC); b2_gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)B2_GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC);
/* Compute maximum speed*/ /* Compute maximum speed*/
b2_gh_max_speed_ref.x= INT_MULT_RSHIFT((int32_t)B2_GH_MAX_SPEED,c_route_ref,INT32_TRIG_FRAC); b2_gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)B2_GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC);
b2_gh_max_speed_ref.y= INT_MULT_RSHIFT((int32_t)B2_GH_MAX_SPEED,s_route_ref,INT32_TRIG_FRAC); b2_gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)B2_GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC);
/* restore b2_gh_speed_ref range (Q14.17) */ /* restore b2_gh_speed_ref range (Q14.17) */
INT32_VECT2_LSHIFT(b2_gh_max_speed_ref, b2_gh_max_speed_ref,B2_GH_SPEED_REF_FRAC - B2_GH_MAX_SPEED_REF_FRAC); INT32_VECT2_LSHIFT(b2_gh_max_speed_ref, b2_gh_max_speed_ref, (B2_GH_SPEED_REF_FRAC - B2_GH_MAX_SPEED_REF_FRAC));
/* Saturate accelerations */ /* Saturate accelerations */
if (b2_gh_accel_ref.x <= -b2_gh_max_accel_ref.x) { if (b2_gh_accel_ref.x <= -b2_gh_max_accel_ref.x) {
b2_gh_accel_ref.x = -b2_gh_max_accel_ref.x; b2_gh_accel_ref.x = -b2_gh_max_accel_ref.x;
@@ -232,7 +243,7 @@ static inline void b2_gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) {
} }
/* Saturate speed and adjust acceleration accordingly */ /* Saturate speed and adjust acceleration accordingly */
if (b2_gh_speed_ref.x <= -b2_gh_max_speed_ref.x) { if (b2_gh_speed_ref.x <= -b2_gh_max_speed_ref.x) {
b2_gh_speed_ref.x = -b2_gh_max_speed_ref.x; b2_gh_speed_ref.x = -b2_gh_max_speed_ref.x;
if (b2_gh_accel_ref.x < 0) if (b2_gh_accel_ref.x < 0)
b2_gh_accel_ref.x = 0; b2_gh_accel_ref.x = 0;