Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
guidance_v_adapt.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2009-2013 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 
31 #include "paparazzi.h"
32 #include "math/pprz_algebra_int.h"
33 #include "generated/airframe.h"
34 
35 
43 #ifndef GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE
44 #ifdef GUIDANCE_V_NOMINAL_HOVER_THROTTLE
45 #define GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE GUIDANCE_V_NOMINAL_HOVER_THROTTLE
46 #else
47 #define GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE 0.3
48 #endif
49 #endif
51 
52 
56 #ifndef GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE
57 #define GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE 0.2
58 #endif
60 
61 
65 #ifndef GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE
66 #define GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE 0.75
67 #endif
69 
70 
75 #ifndef GUIDANCE_V_ADAPT_NOISE_FACTOR
76 #define GUIDANCE_V_ADAPT_NOISE_FACTOR 1.0
77 #endif
78 
79 
83 #ifndef GUIDANCE_V_ADAPT_MAX_ACCEL
84 #define GUIDANCE_V_ADAPT_MAX_ACCEL 4.0
85 #endif
86 
90 #ifndef GUIDANCE_V_ADAPT_MAX_CMD
91 #define GUIDANCE_V_ADAPT_MAX_CMD 0.9
92 #endif
93 #ifndef GUIDANCE_V_ADAPT_MIN_CMD
94 #define GUIDANCE_V_ADAPT_MIN_CMD 0.1
95 #endif
96 
97 
98 
102 
103 /* System noises */
104 #ifndef GV_ADAPT_SYS_NOISE_F
105 #define GV_ADAPT_SYS_NOISE_F 0.00005
106 #endif
107 #define GV_ADAPT_SYS_NOISE BFP_OF_REAL(GV_ADAPT_SYS_NOISE_F, GV_ADAPT_P_FRAC)
108 
109 /* Measuremement noises */
110 #define GV_ADAPT_MEAS_NOISE_HOVER_F (50.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)
111 #define GV_ADAPT_MEAS_NOISE_HOVER BFP_OF_REAL(GV_ADAPT_MEAS_NOISE_HOVER_F, GV_ADAPT_P_FRAC)
112 #define GV_ADAPT_MEAS_NOISE_OF_ZD (100.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)
113 
114 /* Initial Covariance */
115 #define GV_ADAPT_P0_F 0.1
119 
120 void gv_adapt_init(void)
121 {
122  gv_adapt_X = gv_adapt_X0;
123  gv_adapt_P = gv_adapt_P0;
124 }
125 
126 #define K_FRAC 12
127 
133 void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref)
134 {
135 
136  static const int32_t gv_adapt_min_cmd = GUIDANCE_V_ADAPT_MIN_CMD * MAX_PPRZ;
137  static const int32_t gv_adapt_max_cmd = GUIDANCE_V_ADAPT_MAX_CMD * MAX_PPRZ;
138  static const int32_t gv_adapt_max_accel = ACCEL_BFP_OF_REAL(GUIDANCE_V_ADAPT_MAX_ACCEL);
139 
140  /* Update only if accel and commands are in a valid range */
141  /* This also ensures we don't divide by zero */
142  if (thrust_applied < gv_adapt_min_cmd || thrust_applied > gv_adapt_max_cmd
143  || zdd_meas < -gv_adapt_max_accel || zdd_meas > gv_adapt_max_accel) {
144  return;
145  }
146 
147  /* We don't propagate state, it's constant ! */
148  /* We propagate our covariance */
149  gv_adapt_P = gv_adapt_P + GV_ADAPT_SYS_NOISE;
150 
151  /* Compute our measurement. If zdd_meas is in the range +/-5g, meas is less than 30 bits */
152  const int32_t g_m_zdd = ((int32_t)BFP_OF_REAL(9.81,
154  if (g_m_zdd > 0) {
155  gv_adapt_Xmeas = (g_m_zdd + (thrust_applied >> 1)) / thrust_applied;
156  } else {
157  gv_adapt_Xmeas = (g_m_zdd - (thrust_applied >> 1)) / thrust_applied;
158  }
159 
160  /* Compute a residual */
161  int32_t residual = gv_adapt_Xmeas - gv_adapt_X;
162 
163  /* Covariance Error E = P + R */
165  if (zd_ref < 0) { ref = -ref; }
167 
168  /* Kalman gain K = P / (P + R) = P / E */
169  int32_t K = (gv_adapt_P << K_FRAC) / E;
170 
171  /* Update Covariance Pnew = P - K * P */
172  gv_adapt_P = gv_adapt_P - ((K * gv_adapt_P) >> K_FRAC);
173  /* Don't let covariance climb over initial value */
174  if (gv_adapt_P > gv_adapt_P0) {
175  gv_adapt_P = gv_adapt_P0;
176  }
177 
178  /* Update State */
179  gv_adapt_X = gv_adapt_X + ((((int64_t)K * residual)) >> K_FRAC);
180 
181  /* Output bounds.
182  * Don't let it climb over a value that would
183  * give less than #GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE % throttle
184  * or more than #GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE % throttle.
185  */
186  static const int32_t max_out = BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) /
188  static const int32_t min_out = BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) /
190  Bound(gv_adapt_X, min_out, max_out);
191 }
#define GV_ADAPT_P_FRAC
#define K_FRAC
Adaptation block of the vertical guidance.
#define INT32_ACCEL_FRAC
#define GV_ADAPT_SYS_NOISE
#define INT32_SPEED_FRAC
static float K[9]
#define GUIDANCE_V_ADAPT_MAX_CMD
Filter is not fed if command values are out of a % of 0/MAX_PPRZ.
#define GV_ADAPT_MEAS_NOISE_HOVER
#define GV_ADAPT_X_FRAC
signed long long int64_t
Definition: types.h:21
#define GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE
Maximum hover throttle as factor of MAX_PPRZ.
uint16_t ref[TCOUPLE_NB]
#define BFP_OF_REAL(_vr, _frac)
#define GUIDANCE_V_ADAPT_MIN_CMD
static const int32_t gv_adapt_P0
#define GV_ADAPT_MEAS_NOISE_OF_ZD
void gv_adapt_init(void)
static const int32_t gv_adapt_X0
void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref)
Adaptation function.
int32_t gv_adapt_P
Covariance.
signed long int32_t
Definition: types.h:19
int32_t gv_adapt_Xmeas
Measurement.
#define E
#define GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE
Minimum hover throttle as factor of MAX_PPRZ.
#define MAX_PPRZ
Definition: paparazzi.h:8
#define ACCEL_BFP_OF_REAL(_af)
#define GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE
Initial hover throttle as factor of MAX_PPRZ.
#define GV_ADAPT_P0_F
int32_t gv_adapt_X
State of the estimator.
#define GUIDANCE_V_ADAPT_MAX_ACCEL
Filter is not fed if accel values are more than +/- MAX_ACCEL.
Paparazzi fixed point algebra.