Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
guidance_v_ref.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
27 #ifndef GUIDANCE_V_REF_H
28 #define GUIDANCE_V_REF_H
29 
30 #include "generated/airframe.h"
31 #include "inttypes.h"
32 #include "math/pprz_algebra.h"
33 #include "math/pprz_algebra_int.h"
34 
35 /* update frequency */
36 #define GV_FREQ_FRAC 9
37 #define GV_FREQ (1<<GV_FREQ_FRAC)
38 
43 extern int32_t gv_zdd_ref;
44 
46 #define GV_ZDD_REF_FRAC 8
47 
52 extern int32_t gv_zd_ref;
53 
55 #define GV_ZD_REF_FRAC (GV_ZDD_REF_FRAC + GV_FREQ_FRAC)
56 
61 extern int64_t gv_z_ref;
62 
64 #define GV_Z_REF_FRAC (GV_ZD_REF_FRAC + GV_FREQ_FRAC)
65 
66 /* Saturations definition */
67 #ifndef GUIDANCE_V_REF_MIN_ZDD
68 #define GUIDANCE_V_REF_MIN_ZDD (-2.0*9.81)
69 #endif
70 #define GV_MIN_ZDD BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZDD, GV_ZDD_REF_FRAC)
71 
72 #ifndef GUIDANCE_V_REF_MAX_ZDD
73 #define GUIDANCE_V_REF_MAX_ZDD ( 0.8*9.81)
74 #endif
75 #define GV_MAX_ZDD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZDD, GV_ZDD_REF_FRAC)
76 
77 #ifndef GUIDANCE_V_REF_MIN_ZD
78 #define GUIDANCE_V_REF_MIN_ZD (-3.)
79 #endif
80 #define GV_MIN_ZD BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZD , GV_ZD_REF_FRAC)
81 
82 #ifndef GUIDANCE_V_REF_MAX_ZD
83 #define GUIDANCE_V_REF_MAX_ZD ( 3.)
84 #endif
85 #define GV_MAX_ZD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZD , GV_ZD_REF_FRAC)
86 
87 /* second order model natural frequency and damping */
88 #ifndef GUIDANCE_V_REF_OMEGA
89 #define GUIDANCE_V_REF_OMEGA RadOfDeg(100.)
90 #endif
91 #ifndef GUIDANCE_V_REF_ZETA
92 #define GUIDANCE_V_REF_ZETA 0.85
93 #endif
94 #define GV_ZETA_OMEGA_FRAC 10
95 #define GV_ZETA_OMEGA BFP_OF_REAL((GUIDANCE_V_REF_ZETA*GUIDANCE_V_REF_OMEGA), GV_ZETA_OMEGA_FRAC)
96 #define GV_OMEGA_2_FRAC 7
97 #define GV_OMEGA_2 BFP_OF_REAL((GUIDANCE_V_REF_OMEGA*GUIDANCE_V_REF_OMEGA), GV_OMEGA_2_FRAC)
98 
99 /* first order time constant */
100 #define GV_REF_THAU_F 0.25
101 #define GV_REF_INV_THAU_FRAC 16
102 #define GV_REF_INV_THAU BFP_OF_REAL((1./0.25), GV_REF_INV_THAU_FRAC)
103 
104 #ifdef GUIDANCE_V_C
105 static inline void gv_set_ref(int32_t alt, int32_t speed, int32_t accel);
106 static inline void gv_update_ref_from_z_sp(int32_t z_sp);
107 static inline void gv_update_ref_from_zd_sp(int32_t zd_sp);
108 
112 
113 __attribute__ ((always_inline)) static inline void gv_set_ref(int32_t alt, int32_t speed, int32_t accel) {
114  int64_t new_z = ((int64_t)alt)<<(GV_Z_REF_FRAC - INT32_POS_FRAC);
115  gv_z_ref = new_z;
118 }
119 
120 __attribute__ ((always_inline)) static inline void gv_update_ref_from_z_sp(int32_t z_sp) {
121 
122  gv_z_ref += gv_zd_ref;
124 
125  // compute the "speed part" of zdd = -2*zeta*omega*zd -omega^2(z_sp - z)
127  int32_t zdd_speed = ((int32_t)(-2*GV_ZETA_OMEGA)*zd_zdd_res)>>(GV_ZETA_OMEGA_FRAC);
128  // compute z error in z_sp resolution
129  int32_t z_err_sp = z_sp - (int32_t)(gv_z_ref>>(GV_Z_REF_FRAC-INT32_POS_FRAC));
130  // convert to accel resolution
131  int32_t z_err_accel = z_err_sp>>(INT32_POS_FRAC-GV_ZDD_REF_FRAC);
132  int32_t zdd_pos = ((int32_t)(GV_OMEGA_2)*z_err_accel)>>GV_OMEGA_2_FRAC;
133  gv_zdd_ref = zdd_speed + zdd_pos;
134 
135  /* Saturate accelerations */
137 
138  /* Saturate speed and adjust acceleration accordingly */
139  if (gv_zd_ref <= GV_MIN_ZD) {
141  if (gv_zdd_ref < 0)
142  gv_zdd_ref = 0;
143  }
144  else if (gv_zd_ref >= GV_MAX_ZD) {
146  if (gv_zdd_ref > 0)
147  gv_zdd_ref = 0;
148  }
149 }
150 
151 
152 __attribute__ ((always_inline)) static inline void gv_update_ref_from_zd_sp(int32_t zd_sp) {
153 
154  gv_z_ref += gv_zd_ref;
156 
157  int32_t zd_err = gv_zd_ref - (zd_sp>>(INT32_SPEED_FRAC - GV_ZD_REF_FRAC));
158  int32_t zd_err_zdd_res = zd_err>>(GV_ZD_REF_FRAC-GV_ZDD_REF_FRAC);
159  gv_zdd_ref = (-(int32_t)GV_REF_INV_THAU * zd_err_zdd_res)>>GV_REF_INV_THAU_FRAC;
160 
161  /* Saturate accelerations */
162  Bound(gv_zdd_ref, GV_MIN_ZDD, GV_MAX_ZDD);
163 
164  /* Saturate speed and adjust acceleration accordingly */
165  if (gv_zd_ref <= GV_MIN_ZD) {
167  if (gv_zdd_ref < 0)
168  gv_zdd_ref = 0;
169  }
170  else if (gv_zd_ref >= GV_MAX_ZD) {
172  if (gv_zdd_ref > 0)
173  gv_zdd_ref = 0;
174  }
175 }
176 
177 #endif /* GUIDANCE_V_C */
178 
179 #endif /* GUIDANCE_V_REF_H */
int32_t gv_zdd_ref
reference model vertical accel in meters/s^2 (output) fixed point representation with GV_ZDD_REF_FRAC...
#define GV_OMEGA_2
#define GV_ZD_REF_FRAC
number of bits for the fractional part of gv_zd_ref
#define GV_MIN_ZDD
#define GV_Z_REF_FRAC
number of bits for the fractional part of gv_z_ref
int32_t gv_zd_ref
reference model vertical speed in meters/sec (output) fixed point representation with GV_ZD_REF_FRAC ...
#define INT32_POS_FRAC
#define GV_REF_INV_THAU_FRAC
#define GV_ZDD_REF_FRAC
number of bits for the fractional part of gv_zdd_ref
signed long long int64_t
Definition: types.h:21
#define GV_OMEGA_2_FRAC
#define GV_MAX_ZDD
static void gv_update_ref_from_z_sp(int32_t z_sp)
#define GV_REF_INV_THAU
static void gv_set_ref(int32_t alt, int32_t speed, int32_t accel)
#define GV_MAX_ZD
#define GV_ZETA_OMEGA_FRAC
int64_t gv_z_ref
reference model altitude in meters (output) fixed point representation with GV_Z_REF_FRAC Q37...
int32_t int32_t accel
#define GV_ZETA_OMEGA
signed long int32_t
Definition: types.h:19
static void gv_update_ref_from_zd_sp(int32_t zd_sp)
__attribute__((always_inline)) static inline void gv_set_ref(int32_t alt
#define GV_MIN_ZD
#define INT32_SPEED_FRAC
int32_t speed
#define INT32_ACCEL_FRAC
Paparazzi fixed point algebra.