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
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, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  */
22 
23 /*
24  * Runge-Kutta library (float version)
25  */
26 
27 #ifndef PPRZ_RK_FLOAT_H
28 #define PPRZ_RK_FLOAT_H
29 
31 
51 static inline void runge_kutta_1_float(
52  float * xo,
53  const float * x, const int n,
54  const float * u, const int m,
55  void (*f)(float * o, const float * x, const int n, const float * u, const int m),
56  const float dt)
57 {
58  float dx[n];
59 
60  f(dx, x, n, u, m);
61  float_vect_smul(xo, dx, dt, n);
62  float_vect_add(xo, x, n);
63 }
64 
85 static inline void runge_kutta_2_float(
86  float * xo,
87  const float * x, const int n,
88  const float * u, const int m,
89  void (*f)(float * o, const float * x, const int n, const float * u, const int m),
90  const float dt)
91 {
92  float mid_point[n];
93 
94  // mid_point = x + (dt/2)*f(x, u)
95  f(mid_point, x, n, u, m);
96  float_vect_smul(mid_point, mid_point, dt/2., n);
97  float_vect_add(mid_point, x, n);
98  // xo = x + dt * f(mid_point, u)
99  f(xo, mid_point, n, u, m);
100  float_vect_smul(xo, xo, dt, n);
101  float_vect_add(xo, x, n);
102 }
103 
128 static inline void runge_kutta_4_float(
129  float * xo,
130  const float * x, const int n,
131  const float * u, const int m,
132  void (*f)(float * o, const float * x, const int n, const float * u, const int m),
133  const float dt)
134 {
135  float k1[n], k2[n], k3[n], k4[n], ktmp[n];
136 
137  // k1 = f(x, u)
138  f(k1, x, n, u, m);
139 
140  // k2 = f(x + dt * (k1 / 2), u)
141  float_vect_smul(ktmp, k1, dt / 2., n);
142  float_vect_add(ktmp, x, n);
143  f(k2, ktmp, n, u, m);
144 
145  // k3 = f(x + dt * (k2 / 2), u)
146  float_vect_smul(ktmp, k2, dt / 2., n);
147  float_vect_add(ktmp, x, n);
148  f(k3, ktmp, n, u, m);
149 
150  // k4 = f(x + dt * k3, u)
151  float_vect_smul(ktmp, k3, dt, n);
152  float_vect_add(ktmp, x, n);
153  f(k4, ktmp, n, u, m);
154 
155  // xo = x + (dt / 6) * (k1 + 2 * (k2 + k3) + k4)
156  float_vect_add(k2, k3, n);
157  float_vect_smul(k2, k2, 2., n);
158  float_vect_add(k1, k2, n);
159  float_vect_add(k1, k4, n);
160  float_vect_smul(k1, k1, dt / 6., n);
161  float_vect_sum(xo, x, k1, n);
162 }
163 
164 #endif
165 
static const float dt
static void float_vect_sum(float *o, const float *a, const float *b, const int n)
o = a + b
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:51
Paparazzi floating point algebra.
static void float_vect_add(float *a, const float *b, const int n)
a += b
static void float_vect_smul(float *o, const float *a, const float s, const int n)
o = a * s
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.
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:85