Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
guidance_v_adpt.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2009-2010 The Paparazzi Team
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 
30 #ifndef GUIDANCE_V_ADPT
31 #define GUIDANCE_V_ADPT
32 
33 #include "paparazzi.h"
34 
40 #ifndef GUIDANCE_V_ADAPT_NOISE_FACTOR
41 #define GUIDANCE_V_ADAPT_NOISE_FACTOR 1.0
42 #endif
43 
49 #ifndef GUIDANCE_V_ADAPT_X0
50 #define GUIDANCE_V_ADAPT_X0 0.003
51 #endif
52 
56 #ifndef GUIDANCE_V_ADAPT_MAX_ACCEL
57 #define GUIDANCE_V_ADAPT_MAX_ACCEL 4.0
58 #endif
59 
63 #ifndef GUIDANCE_V_ADAPT_MAX_CMD
64 #define GUIDANCE_V_ADAPT_MAX_CMD 0.9
65 #endif
66 #ifndef GUIDANCE_V_ADAPT_MIN_CMD
67 #define GUIDANCE_V_ADAPT_MIN_CMD 0.1
68 #endif
69 
74 extern int32_t gv_adapt_X;
75 #define GV_ADAPT_X_FRAC 24
76 
81 extern int32_t gv_adapt_P;
82 #define GV_ADAPT_P_FRAC 18
83 
85 extern int32_t gv_adapt_Xmeas;
86 
87 
88 #ifdef GUIDANCE_V_C
89 
93 
94 
95 /* Initial State and Covariance */
96 #define GV_ADAPT_X0_F GUIDANCE_V_ADAPT_X0
97 #define GV_ADAPT_X0 BFP_OF_REAL(GV_ADAPT_X0_F, GV_ADAPT_X_FRAC)
98 #define GV_ADAPT_P0_F 0.1
99 #define GV_ADAPT_P0 BFP_OF_REAL(GV_ADAPT_P0_F, GV_ADAPT_P_FRAC)
100 
101 /* System noises */
102 #ifndef GV_ADAPT_SYS_NOISE_F
103 #define GV_ADAPT_SYS_NOISE_F 0.00005
104 #endif
105 #define GV_ADAPT_SYS_NOISE BFP_OF_REAL(GV_ADAPT_SYS_NOISE_F, GV_ADAPT_P_FRAC)
106 
107 /* Measuremement noises */
108 #define GV_ADAPT_MEAS_NOISE_HOVER_F (50.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)
109 #define GV_ADAPT_MEAS_NOISE_HOVER BFP_OF_REAL(GV_ADAPT_MEAS_NOISE_HOVER_F, GV_ADAPT_P_FRAC)
110 #define GV_ADAPT_MEAS_NOISE_OF_ZD (100.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)
111 
112 /* Max accel */
113 #define GV_ADAPT_MAX_ACCEL ACCEL_BFP_OF_REAL(GUIDANCE_V_ADAPT_MAX_ACCEL)
114 
115 /* Command bounds */
116 #define GV_ADAPT_MAX_CMD ((int32_t)(GUIDANCE_V_ADAPT_MAX_CMD*MAX_PPRZ))
117 #define GV_ADAPT_MIN_CMD ((int32_t)(GUIDANCE_V_ADAPT_MIN_CMD*MAX_PPRZ))
118 
119 /* Output bounds.
120  * Don't let it climb over a value that would
121  * give less than zero throttle and don't let it down to zero.
122  * Worst cases:
123  * MIN_ACCEL / MAX_THROTTLE
124  * MAX_ACCEL / MIN_THROTTLE
125  * ex:
126  * 9.81*2^18/255 = 10084
127  * 9.81*2^18/1 = 2571632
128  */
129 // TODO Check this properly
130 #define GV_ADAPT_MAX_OUT (BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC))
131 #define GV_ADAPT_MIN_OUT (BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) / MAX_PPRZ)
132 
133 
134 static inline void gv_adapt_init(void) {
137 }
138 
139 #define K_FRAC 12
140 
146 static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref) {
147 
148  /* Update only if accel and commands are in a valid range */
149  /* This also ensures we don't divide by zero */
150  if (thrust_applied < GV_ADAPT_MIN_CMD || thrust_applied > GV_ADAPT_MAX_CMD
151  || zdd_meas < -GV_ADAPT_MAX_ACCEL || zdd_meas > GV_ADAPT_MAX_ACCEL) {
152  return;
153  }
154 
155  /* We don't propagate state, it's constant ! */
156  /* We propagate our covariance */
158 
159  /* Compute our measurement. If zdd_meas is in the range +/-5g, meas is less than 30 bits */
160  const int32_t g_m_zdd = ((int32_t)BFP_OF_REAL(9.81, INT32_ACCEL_FRAC) - zdd_meas)<<(GV_ADAPT_X_FRAC - INT32_ACCEL_FRAC);
161  if ( g_m_zdd > 0) {
162  gv_adapt_Xmeas = (g_m_zdd + (thrust_applied>>1)) / thrust_applied;
163  } else {
164  gv_adapt_Xmeas = (g_m_zdd - (thrust_applied>>1)) / thrust_applied;
165  }
166 
167  /* Compute a residual */
168  int32_t residual = gv_adapt_Xmeas - gv_adapt_X;
169 
170  /* Covariance Error E = P + R */
172  if (zd_ref < 0) ref = -ref;
174 
175  /* Kalman gain K = P / (P + R) = P / E */
176  int32_t K = (gv_adapt_P<<K_FRAC) / E;
177 
178  /* Update Covariance Pnew = P - K * P */
179  gv_adapt_P = gv_adapt_P - ((K * gv_adapt_P)>>K_FRAC);
180  /* Don't let covariance climb over initial value */
182 
183  /* Update State */
184  gv_adapt_X = gv_adapt_X + (((int64_t)(K * residual))>>K_FRAC);
185 
186  /* Again don't let it climb over a value that would
187  * give less than zero throttle and don't let it down to zero.
188  */
189  Bound(gv_adapt_X, GV_ADAPT_MIN_OUT, GV_ADAPT_MAX_OUT);
190 }
191 
192 
193 #endif /* GUIDANCE_V_C */
194 
195 #endif /* GUIDANCE_V_ADPT */
#define GV_ADAPT_MEAS_NOISE_HOVER
static void gv_adapt_init(void)
#define GV_ADAPT_X0
int32_t gv_adapt_X
State of the estimator.
signed long long int64_t
Definition: types.h:21
#define GV_ADAPT_P0
#define E
int32_t gv_adapt_Xmeas
Measurement.
#define GV_ADAPT_MIN_OUT
uint16_t ref[TCOUPLE_NB]
#define GV_ADAPT_MAX_ACCEL
#define GV_ADAPT_MAX_OUT
#define GV_ADAPT_P_FRAC
int32_t gv_adapt_P
Covariance.
#define BFP_OF_REAL(_vr, _frac)
static void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref)
Adaptation function.
signed long int32_t
Definition: types.h:19
#define GV_ADAPT_MEAS_NOISE_OF_ZD
#define GV_ADAPT_SYS_NOISE
#define INT32_SPEED_FRAC
#define GV_ADAPT_MAX_CMD
#define INT32_ACCEL_FRAC
#define K_FRAC
#define GV_ADAPT_X_FRAC