Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
47 int64_t gv_z_ref;
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 10.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 {
89  int64_t new_z = ((int64_t)alt) << (GV_Z_REF_FRAC - INT32_POS_FRAC);
90  gv_z_ref = new_z;
93 }
94 
96 {
97 
100 
101  // compute the "speed part" of zdd = -2*zeta*omega*zd -omega^2(z_sp - z)
102  int32_t zd_zdd_res = gv_zd_ref >> (GV_ZD_REF_FRAC - GV_ZDD_REF_FRAC);
103  int32_t zdd_speed = ((int32_t)(-2 * GV_ZETA_OMEGA) * zd_zdd_res) >> (GV_ZETA_OMEGA_FRAC);
104  // compute z error in z_sp resolution
105  int32_t z_err_sp = z_sp - (int32_t)(gv_z_ref >> (GV_Z_REF_FRAC - INT32_POS_FRAC));
106  // convert to accel resolution
107  int32_t z_err_accel = z_err_sp >> (INT32_POS_FRAC - GV_ZDD_REF_FRAC);
108  int32_t zdd_pos = ((int32_t)(GV_OMEGA_2) * z_err_accel) >> GV_OMEGA_2_FRAC;
109  gv_zdd_ref = zdd_speed + zdd_pos;
110 
111  /* Saturate accelerations */
113 
114  /* Saturate speed and adjust acceleration accordingly */
115  if (gv_zd_ref <= GV_MIN_ZD) {
117  if (gv_zdd_ref < 0) {
118  gv_zdd_ref = 0;
119  }
120  } else if (gv_zd_ref >= GV_MAX_ZD) {
122  if (gv_zdd_ref > 0) {
123  gv_zdd_ref = 0;
124  }
125  }
126 }
127 
128 
130 {
131 
132  gv_z_ref += gv_zd_ref;
134 
135  /* limit z_ref to GUIDANCE_V_REF_MAX_Z_DIFF from current z pos */
136  int64_t cur_z = ((int64_t)z_pos) << (GV_Z_REF_FRAC - INT32_POS_FRAC);
137  Bound(gv_z_ref, cur_z - GV_MAX_Z_DIFF, cur_z + GV_MAX_Z_DIFF);
138 
139  int32_t zd_err = gv_zd_ref - (zd_sp >> (INT32_SPEED_FRAC - GV_ZD_REF_FRAC));
140  int32_t zd_err_zdd_res = zd_err >> (GV_ZD_REF_FRAC - GV_ZDD_REF_FRAC);
141  gv_zdd_ref = (-(int32_t)GV_REF_INV_THAU * zd_err_zdd_res) >> GV_REF_INV_THAU_FRAC;
142 
143  /* Saturate accelerations */
145 
146  /* Saturate speed and adjust acceleration accordingly */
147  if (gv_zd_ref <= GV_MIN_ZD) {
149  if (gv_zdd_ref < 0) {
150  gv_zdd_ref = 0;
151  }
152  } else if (gv_zd_ref >= GV_MAX_ZD) {
154  if (gv_zdd_ref > 0) {
155  gv_zdd_ref = 0;
156  }
157  }
158 }
159 
#define INT32_POS_FRAC
#define INT32_SPEED_FRAC
#define INT32_ACCEL_FRAC
#define GV_ZETA_OMEGA
#define GV_MAX_ZDD
int32_t gv_zdd_ref
reference model vertical accel in meters/s^2 (output) fixed point representation with GV_ZDD_REF_FRAC...
#define GV_MAX_ZD
#define GV_MAX_Z_DIFF
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 GV_MIN_ZDD
#define GV_OMEGA_2_FRAC
void gv_set_ref(int32_t alt, int32_t speed, int32_t accel)
void gv_update_ref_from_z_sp(int32_t z_sp)
void gv_update_ref_from_zd_sp(int32_t zd_sp, int32_t z_pos)
update vertical reference from speed setpoint.
#define GV_OMEGA_2
#define GV_REF_INV_THAU_FRAC
#define GV_MIN_ZD
#define GV_ZETA_OMEGA_FRAC
int32_t gv_zd_ref
reference model vertical speed in meters/sec (output) fixed point representation with GV_ZD_REF_FRAC ...
Reference generation for vertical guidance.
#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_ZDD_REF_FRAC
number of bits for the fractional part of gv_zdd_ref
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83