Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
pprz_rk_float.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 Gautier Hattenberger
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
27 #ifndef PPRZ_RK_FLOAT_H
28 #define PPRZ_RK_FLOAT_H
29 
30 #ifdef __cplusplus
31 extern "C" {
32 #endif
33 
35 
55 static inline void runge_kutta_1_float(
56  float *xo,
57  const float *x, const int n,
58  const float *u, const int m,
59  void (*f)(float *o, const float *x, const int n, const float *u, const int m),
60  const float dt)
61 {
62  float dx[n];
63 
64  f(dx, x, n, u, m);
65  float_vect_smul(xo, dx, dt, n);
66  float_vect_add(xo, x, n);
67 }
68 
89 static inline void runge_kutta_2_float(
90  float *xo,
91  const float *x, const int n,
92  const float *u, const int m,
93  void (*f)(float *o, const float *x, const int n, const float *u, const int m),
94  const float dt)
95 {
96  float mid_point[n];
97 
98  // mid_point = x + (dt/2)*f(x, u)
99  f(mid_point, x, n, u, m);
100  float_vect_smul(mid_point, mid_point, dt / 2., n);
101  float_vect_add(mid_point, x, n);
102  // xo = x + dt * f(mid_point, u)
103  f(xo, mid_point, n, u, m);
104  float_vect_smul(xo, xo, dt, n);
105  float_vect_add(xo, x, n);
106 }
107 
132 static inline void runge_kutta_4_float(
133  float *xo,
134  const float *x, const int n,
135  const float *u, const int m,
136  void (*f)(float *o, const float *x, const int n, const float *u, const int m),
137  const float dt)
138 {
139  float k1[n], k2[n], k3[n], k4[n], ktmp[n];
140 
141  // k1 = f(x, u)
142  f(k1, x, n, u, m);
143 
144  // k2 = f(x + dt * (k1 / 2), u)
145  float_vect_smul(ktmp, k1, dt / 2., n);
146  float_vect_add(ktmp, x, n);
147  f(k2, ktmp, n, u, m);
148 
149  // k3 = f(x + dt * (k2 / 2), u)
150  float_vect_smul(ktmp, k2, dt / 2., n);
151  float_vect_add(ktmp, x, n);
152  f(k3, ktmp, n, u, m);
153 
154  // k4 = f(x + dt * k3, u)
155  float_vect_smul(ktmp, k3, dt, n);
156  float_vect_add(ktmp, x, n);
157  f(k4, ktmp, n, u, m);
158 
159  // xo = x + (dt / 6) * (k1 + 2 * (k2 + k3) + k4)
160  float_vect_add(k2, k3, n);
161  float_vect_smul(k2, k2, 2., n);
162  float_vect_add(k1, k2, n);
163  float_vect_add(k1, k4, n);
164  float_vect_smul(k1, k1, dt / 6., n);
165  float_vect_sum(xo, x, k1, n);
166 }
167 
168 #ifdef __cplusplus
169 } /* extern "C" */
170 #endif
171 
172 #endif /* PPRZ_RK_FLOAT_H */
float_vect_add
static void float_vect_add(float *a, const float *b, const int n)
a += b
Definition: pprz_algebra_float.h:574
runge_kutta_2_float
static void runge_kutta_2_float(float *xo, const float *x, const int n, const float *u, const int m, void(*f)(float *o, const float *x, const int n, const float *u, const int m), const float dt)
Second-Order Runge-Kutta.
Definition: pprz_rk_float.h:89
pprz_algebra_float.h
Paparazzi floating point algebra.
runge_kutta_1_float
static void runge_kutta_1_float(float *xo, const float *x, const int n, const float *u, const int m, void(*f)(float *o, const float *x, const int n, const float *u, const int m), const float dt)
First-Order Runge-Kutta.
Definition: pprz_rk_float.h:55
runge_kutta_4_float
static void runge_kutta_4_float(float *xo, const float *x, const int n, const float *u, const int m, void(*f)(float *o, const float *x, const int n, const float *u, const int m), const float dt)
Fourth-Order Runge-Kutta.
Definition: pprz_rk_float.h:132
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
float_vect_smul
static void float_vect_smul(float *o, const float *a, const float s, const int n)
o = a * s
Definition: pprz_algebra_float.h:588
float_vect_sum
static void float_vect_sum(float *o, const float *a, const float *b, const int n)
o = a + b
Definition: pprz_algebra_float.h:553