flight_path.c
3.39 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
/*
* flight_path.c, calculate a 3-D cubic spline flight path
*
* could be more efficient if quaternions were used?
*/
#include "flight_path.h"
/*
* Enter your flight path way points here
*/
static float myp[] = {0, 15, 31};
static float vpx[] = {50, -40, 10};
static float vpy[] = {300, 30, 300};
static float vpz[] = {-600, 0, 600};
static float atx[] = {0, -110, 0};
static float aty[] = {50, 10, 0};
static float atz[] = {0, 195, 0};
static float upx[] = {0, -0.173648, 0.173648};
static float upy[] = {1, 0.9848077, 0.9848077};
static float upz[] = {0, 0, 0};
#define ARRAY_SIZE ((sizeof(vpx)) / (sizeof(float)))
/* path coefficients */
static float vpx_c[ARRAY_SIZE];
static float vpy_c[ARRAY_SIZE];
static float vpz_c[ARRAY_SIZE];
static float atx_c[ARRAY_SIZE];
static float aty_c[ARRAY_SIZE];
static float atz_c[ARRAY_SIZE];
static float upx_c[ARRAY_SIZE];
static float upy_c[ARRAY_SIZE];
static float upz_c[ARRAY_SIZE];
/*
* Determine cubic coefficients for set of points given in
* t,y. Results returned in z. Number of points is n.
*/
static void
find_spline_coeffs( int n, float *t, float *y, float *z)
{
float h[MAX_FP_SIZE],
b[MAX_FP_SIZE],
u[MAX_FP_SIZE],
v[MAX_FP_SIZE];
int i;
for(i = 0; i < n; i++)
{
h[i] = t[i+1] - t[i];
b[i] = (y[i+1] - y[i]) / h[i];
}
u[1] = 2.0 * (h[0] + h[1]);
v[1] = 6.0 * (b[1] - b[0]);
for( i = 2; i < n-1; i++)
{
u[i] = 2.0 * (h[i] + h[i-1]) - (h[i-1] * h[i-1]) / u[i-1];
v[i] = 6.0 * (b[i] - b[i-1]) - (h[i-1] * v[i-1]) / u[i-1];
}
z[n-1] = 0.0;
for(i = n-2; i >= 1; i--)
z[i] = (v[i] - h[i] * z[i+1]) / u[i];
z[0] = 0.0;
}
/*
* Evaluate cubic spline at given point.
* First determines the interval in t containing x and then
* returns Si(x) using nested form of cubic polynomial.
*/
static float
eval_spline(int n, float *t, float *y, float *z, float x)
{
float h, b, p, diff;
int i;
for(i = n-2; i >= 0; i--)
{
diff = x - t[i];
if(diff >= 0.0)
break;
}
h = t[i+1] - t[i];
b = ((y[i+1] - y[i]) / h) - h * (z[i+1] + 2.0 * z[i]) / 6.0;
p = 0.5 * z[i] + diff * (z[i+1] - z[i]) / (6.0 * h);
p = b + diff * p;
return(y[i] + diff * p);
}
void
get_coefficients( void )
{
find_spline_coeffs( ARRAY_SIZE, myp, vpx, vpx_c);
find_spline_coeffs( ARRAY_SIZE, myp, vpy, vpy_c);
find_spline_coeffs( ARRAY_SIZE, myp, vpz, vpz_c);
find_spline_coeffs( ARRAY_SIZE, myp, atx, atx_c);
find_spline_coeffs( ARRAY_SIZE, myp, aty, aty_c);
find_spline_coeffs( ARRAY_SIZE, myp, atz, atz_c);
find_spline_coeffs( ARRAY_SIZE, myp, upx, upx_c);
find_spline_coeffs( ARRAY_SIZE, myp, upy, upy_c);
find_spline_coeffs( ARRAY_SIZE, myp, upz, upz_c);
}
void
get_fp_position( float p,
float *vx, float *vy, float *vz,
float *ax, float *ay, float *az,
float *ux, float *uy, float *uz)
{
*vx = eval_spline(ARRAY_SIZE, myp, vpx, vpx_c, p);
*vy = eval_spline(ARRAY_SIZE, myp, vpy, vpy_c, p);
*vz = eval_spline(ARRAY_SIZE, myp, vpz, vpz_c, p);
*ax = eval_spline(ARRAY_SIZE, myp, atx, atx_c, p);
*ay = eval_spline(ARRAY_SIZE, myp, aty, aty_c, p);
*az = eval_spline(ARRAY_SIZE, myp, atz, atz_c, p);
*ux = eval_spline(ARRAY_SIZE, myp, upx, upx_c, p);
*uy = eval_spline(ARRAY_SIZE, myp, upy, upy_c, p);
*uz = eval_spline(ARRAY_SIZE, myp, upz, upz_c, p);
}