Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
guidance_v_ref.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
3  * Copyright (C) 2013 Gautier Hattenberger
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
29 #include "generated/airframe.h"
30 
36 
42 
48 
49 
50 /* Saturations definition */
51 #ifndef GUIDANCE_V_REF_MIN_ZDD
52 #define GUIDANCE_V_REF_MIN_ZDD (-2.0*9.81)
53 #endif
54 #define GV_MIN_ZDD BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZDD, GV_ZDD_REF_FRAC)
55 
56 #ifndef GUIDANCE_V_REF_MAX_ZDD
57 #define GUIDANCE_V_REF_MAX_ZDD ( 0.8*9.81)
58 #endif
59 #define GV_MAX_ZDD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZDD, GV_ZDD_REF_FRAC)
60 
62 #ifndef GUIDANCE_V_REF_MAX_Z_DIFF
63 #define GUIDANCE_V_REF_MAX_Z_DIFF 2.0
64 #endif
65 #define GV_MAX_Z_DIFF BFP_OF_REAL(GUIDANCE_V_REF_MAX_Z_DIFF, GV_Z_REF_FRAC)
66 
67 #define GV_MIN_ZD BFP_OF_REAL(GUIDANCE_V_REF_MIN_ZD , GV_ZD_REF_FRAC)
68 #define GV_MAX_ZD BFP_OF_REAL(GUIDANCE_V_REF_MAX_ZD , GV_ZD_REF_FRAC)
69 
70 /* second order model natural frequency and damping */
71 #ifndef GUIDANCE_V_REF_OMEGA
72 #define GUIDANCE_V_REF_OMEGA RadOfDeg(100.)
73 #endif
74 #ifndef GUIDANCE_V_REF_ZETA
75 #define GUIDANCE_V_REF_ZETA 0.85
76 #endif
77 #define GV_ZETA_OMEGA_FRAC 10
78 #define GV_ZETA_OMEGA BFP_OF_REAL((GUIDANCE_V_REF_ZETA*GUIDANCE_V_REF_OMEGA), GV_ZETA_OMEGA_FRAC)
79 #define GV_OMEGA_2_FRAC 7
80 #define GV_OMEGA_2 BFP_OF_REAL((GUIDANCE_V_REF_OMEGA*GUIDANCE_V_REF_OMEGA), GV_OMEGA_2_FRAC)
81 
82 /* first order time constant */
83 #define GV_REF_THAU_F 0.25
84 #define GV_REF_INV_THAU_FRAC 16
85 #define GV_REF_INV_THAU BFP_OF_REAL((1./0.25), GV_REF_INV_THAU_FRAC)
86 
87 void gv_set_ref(int32_t alt, int32_t speed, int32_t accel) {
88  int64_t new_z = ((int64_t)alt)<<(GV_Z_REF_FRAC - INT32_POS_FRAC);
89  gv_z_ref = new_z;
92 }
93 
95 
98 
99  // compute the "speed part" of zdd = -2*zeta*omega*zd -omega^2(z_sp - z)
101  int32_t zdd_speed = ((int32_t)(-2*GV_ZETA_OMEGA)*zd_zdd_res)>>(GV_ZETA_OMEGA_FRAC);
102  // compute z error in z_sp resolution
103  int32_t z_err_sp = z_sp - (int32_t)(gv_z_ref>>(GV_Z_REF_FRAC-INT32_POS_FRAC));
104  // convert to accel resolution
105  int32_t z_err_accel = z_err_sp>>(INT32_POS_FRAC-GV_ZDD_REF_FRAC);
106  int32_t zdd_pos = ((int32_t)(GV_OMEGA_2)*z_err_accel)>>GV_OMEGA_2_FRAC;
107  gv_zdd_ref = zdd_speed + zdd_pos;
108 
109  /* Saturate accelerations */
111 
112  /* Saturate speed and adjust acceleration accordingly */
113  if (gv_zd_ref <= GV_MIN_ZD) {
115  if (gv_zdd_ref < 0)
116  gv_zdd_ref = 0;
117  }
118  else if (gv_zd_ref >= GV_MAX_ZD) {
120  if (gv_zdd_ref > 0)
121  gv_zdd_ref = 0;
122  }
123 }
124 
125 
127 
128  gv_z_ref += gv_zd_ref;
130 
131  /* limit z_ref to GUIDANCE_V_REF_MAX_Z_DIFF from current z pos */
132  int64_t cur_z = ((int64_t)z_pos) << (GV_Z_REF_FRAC - INT32_POS_FRAC);
133  Bound(gv_z_ref, cur_z - GV_MAX_Z_DIFF, cur_z + GV_MAX_Z_DIFF);
134 
135  int32_t zd_err = gv_zd_ref - (zd_sp>>(INT32_SPEED_FRAC - GV_ZD_REF_FRAC));
136  int32_t zd_err_zdd_res = zd_err>>(GV_ZD_REF_FRAC-GV_ZDD_REF_FRAC);
137  gv_zdd_ref = (-(int32_t)GV_REF_INV_THAU * zd_err_zdd_res)>>GV_REF_INV_THAU_FRAC;
138 
139  /* Saturate accelerations */
140  Bound(gv_zdd_ref, GV_MIN_ZDD, GV_MAX_ZDD);
141 
142  /* Saturate speed and adjust acceleration accordingly */
143  if (gv_zd_ref <= GV_MIN_ZD) {
145  if (gv_zdd_ref < 0)
146  gv_zdd_ref = 0;
147  }
148  else if (gv_zd_ref >= GV_MAX_ZD) {
150  if (gv_zdd_ref > 0)
151  gv_zdd_ref = 0;
152  }
153 }
154 
#define GV_ZD_REF_FRAC
number of bits for the fractional part of gv_zd_ref
#define GV_Z_REF_FRAC
number of bits for the fractional part of gv_z_ref
#define GV_MAX_ZDD
#define INT32_POS_FRAC
#define GV_OMEGA_2_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
void gv_update_ref_from_z_sp(int32_t z_sp)
#define GV_ZETA_OMEGA
#define GV_MIN_ZDD
int32_t gv_zd_ref
reference model vertical speed in meters/sec (output) fixed point representation with GV_ZD_REF_FRAC ...
#define GV_ZETA_OMEGA_FRAC
void gv_update_ref_from_zd_sp(int32_t zd_sp, int32_t z_pos)
update vertical reference from speed setpoint.
signed long int32_t
Definition: types.h:19
#define GV_OMEGA_2
void gv_set_ref(int32_t alt, int32_t speed, int32_t accel)
#define GV_MIN_ZD
Reference generation for vertical guidance.
int64_t gv_z_ref
reference model altitude in meters (output) fixed point representation with GV_Z_REF_FRAC Q37...
#define GV_REF_INV_THAU
#define INT32_SPEED_FRAC
#define GV_MAX_ZD
#define GV_MAX_Z_DIFF
#define GV_REF_INV_THAU_FRAC
#define INT32_ACCEL_FRAC
int32_t gv_zdd_ref
reference model vertical accel in meters/s^2 (output) fixed point representation with GV_ZDD_REF_FRAC...