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_h_ref.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 ENAC <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_H_REF_H
28 #define GUIDANCE_H_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 B2_GH_FREQ_FRAC 9
37 #define B2_GH_FREQ (1<<B2_GH_FREQ_FRAC)
38 
39 /* reference model accel in meters/sec2 (output) */
40 /* Q23.8 : accuracy 0.0039 , range 8388km/s2 */
41 /* int32_4_8_t */
42 extern struct Int32Vect2 b2_gh_accel_ref;
43 #define B2_GH_ACCEL_REF_FRAC 8
44 
45 /* reference model speed in meters/sec (output) */
46 /* Q14.17 : accuracy 0.0000076 , range 16384m/s2 */
47 extern struct Int32Vect2 b2_gh_speed_ref;
48 #define B2_GH_SPEED_REF_FRAC (B2_GH_ACCEL_REF_FRAC + B2_GH_FREQ_FRAC)
49 
50 /* reference model position in meters (output) */
51 /* Q37.26 : */
52 extern struct Int64Vect2 b2_gh_pos_ref;
53 #define B2_GH_POS_REF_FRAC (B2_GH_SPEED_REF_FRAC + B2_GH_FREQ_FRAC)
54 
55 /* Saturations definition */
56 #ifndef GUIDANCE_H_REF_MAX_ACCEL
57 #define GUIDANCE_H_REF_MAX_ACCEL ( tanf(RadOfDeg(30.))*9.81 )
58 #endif
59 #define B2_GH_MAX_ACCEL BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, B2_GH_ACCEL_REF_FRAC)
60 
61 #ifndef GUIDANCE_H_REF_MAX_SPEED
62 #define GUIDANCE_H_REF_MAX_SPEED ( 5. )
63 #endif
64 #define B2_GH_MAX_SPEED BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, B2_GH_SPEED_REF_FRAC)
65 
66 /* second order model natural frequency and damping */
67 #ifndef GUIDANCE_H_REF_OMEGA
68 #define GUIDANCE_H_REF_OMEGA RadOfDeg(67.)
69 #endif
70 #ifndef GUIDANCE_H_REF_ZETA
71 #define GUIDANCE_H_REF_ZETA 0.85
72 #endif
73 #define B2_GH_ZETA_OMEGA_FRAC 10
74 #define B2_GH_ZETA_OMEGA BFP_OF_REAL((GUIDANCE_H_REF_ZETA*GUIDANCE_H_REF_OMEGA), B2_GH_ZETA_OMEGA_FRAC)
75 #define B2_GH_OMEGA_2_FRAC 7
76 #define B2_GH_OMEGA_2 BFP_OF_REAL((GUIDANCE_H_REF_OMEGA*GUIDANCE_H_REF_OMEGA), B2_GH_OMEGA_2_FRAC)
77 
78 /* first order time constant */
79 #define B2_GH_REF_THAU_F 0.5
80 #define B2_GH_REF_INV_THAU_FRAC 16
81 #define B2_GH_REF_INV_THAU BFP_OF_REAL((1./B2_GH_REF_THAU_F), B2_GH_REF_INV_THAU_FRAC)
82 
83 #ifdef GUIDANCE_H_C
84 static inline void b2_gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel);
85 static inline void b2_gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp);
86 static inline void b2_gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp);
87 
91 
92 static inline void b2_gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel) {
93  struct Int64Vect2 new_pos;
94  new_pos.x = ((int64_t)pos.x)<<(B2_GH_POS_REF_FRAC - INT32_POS_FRAC);
95  new_pos.y = ((int64_t)pos.y)<<(B2_GH_POS_REF_FRAC - INT32_POS_FRAC);
96  b2_gh_pos_ref = new_pos;
99 }
100 
101 static inline void b2_gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) {
102 
105 
106  // compute the "speed part" of accel = -2*zeta*omega*speed -omega^2(pos - pos_sp)
107  struct Int32Vect2 speed;
109  VECT2_SMUL(speed, speed, -2*B2_GH_ZETA_OMEGA);
111  // compute pos error in pos_sp resolution
112  struct Int32Vect2 pos_err;
114  VECT2_DIFF(pos_err, pos_err, pos_sp);
115  // convert to accel resolution
117  // compute the "pos part" of accel
118  struct Int32Vect2 pos;
119  VECT2_SMUL(pos, pos_err, (-B2_GH_OMEGA_2));
121  // sum accel
122  VECT2_SUM(b2_gh_accel_ref, speed, pos);
123 
124  /* Saturate accelerations */
126 
127  /* Saturate speed and adjust acceleration accordingly */
130  if (b2_gh_accel_ref.x < 0)
131  b2_gh_accel_ref.x = 0;
132  }
133  else if (b2_gh_speed_ref.x >= B2_GH_MAX_SPEED) {
135  if (b2_gh_accel_ref.x > 0)
136  b2_gh_accel_ref.x = 0;
137  }
140  if (b2_gh_accel_ref.y < 0)
141  b2_gh_accel_ref.y = 0;
142  }
143  else if (b2_gh_speed_ref.y >= B2_GH_MAX_SPEED) {
145  if (b2_gh_accel_ref.y > 0)
146  b2_gh_accel_ref.y = 0;
147  }
148 }
149 
150 
151 static inline void b2_gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) {
152 
155 
156  // compute speed error
157  struct Int32Vect2 speed_err;
158  INT32_VECT2_RSHIFT(speed_err, speed_sp, (INT32_SPEED_FRAC - B2_GH_SPEED_REF_FRAC));
159  VECT2_DIFF(speed_err, b2_gh_speed_ref, speed_err);
160  // convert to accel resolution
162  // compute accel from speed_sp
165 
166  /* Saturate accelerations */
168 
169  /* Saturate speed and adjust acceleration accordingly */
172  if (b2_gh_accel_ref.x < 0)
173  b2_gh_accel_ref.x = 0;
174  }
175  else if (b2_gh_speed_ref.x >= B2_GH_MAX_SPEED) {
177  if (b2_gh_accel_ref.x > 0)
178  b2_gh_accel_ref.x = 0;
179  }
182  if (b2_gh_accel_ref.y < 0)
183  b2_gh_accel_ref.y = 0;
184  }
185  else if (b2_gh_speed_ref.y >= B2_GH_MAX_SPEED) {
187  if (b2_gh_accel_ref.y > 0)
188  b2_gh_accel_ref.y = 0;
189  }
190 }
191 
192 #endif /* GUIDANCE_H_C */
193 
194 #endif /* GUIDANCE_H_REF_H */
#define INT32_VECT2_RSHIFT(_o, _i, _r)
#define INT32_POS_FRAC
#define B2_GH_ACCEL_REF_FRAC
struct Int32Vect2 b2_gh_speed_ref
struct Int32Vect2 b2_gh_accel_ref
signed long long int64_t
Definition: types.h:21
#define B2_GH_OMEGA_2
#define VECT2_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:69
struct Int64Vect2 b2_gh_pos_ref
#define B2_GH_MAX_ACCEL
#define B2_GH_MAX_SPEED
int32_t int32_t accel
#define B2_GH_REF_INV_THAU_FRAC
#define B2_GH_REF_INV_THAU
#define VECT2_SUM(_c, _a, _b)
Definition: pprz_algebra.h:63
#define VECT2_ADD(_a, _b)
Definition: pprz_algebra.h:51
#define B2_GH_SPEED_REF_FRAC
#define B2_GH_ZETA_OMEGA_FRAC
#define B2_GH_POS_REF_FRAC
#define VECT2_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:75
#define B2_GH_ZETA_OMEGA
#define INT32_SPEED_FRAC
int32_t speed
#define B2_GH_OMEGA_2_FRAC
#define INT32_ACCEL_FRAC
#define VECT2_STRIM(_v, _min, _max)
Definition: pprz_algebra.h:87
Paparazzi fixed point algebra.