Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
gvf_ik.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2016 Hector Garcia de Marina
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#include <math.h>
24#include "std.h"
25
26#include "gvf_ik.h"
27#include "autopilot.h"
28
29/* TODO:
30 * - Reset gamma_t0 whenever any gamma setting is updated.
31 * - The GVF drawn in the GCS is not the IK-GVF, so it needs to be fixed.
32 */
33
34// Control
37
38// Time variables to check if GVF is active
39static uint32_t gamma_t0 = 0;
41
43
44#if PERIODIC_TELEMETRY
46static void send_gvf(struct transport_tx *trans, struct link_device *dev)
47{
48
49 pprz_msg_send_GVF_IK(
50 trans, dev, AC_ID,
51 &gvf_ik_control.error,
52 &gvf_ik_control.gamma,
53 &gvf_ik_control.gamma_dot,
54 &gvf_ik_control.gamma_amplitude,
55 &gvf_ik_control.gamma_omega
56 );
57
59 if (delta_T < 200) {
60 uint8_t traj_type = (uint8_t)gvf_trajectory.type;
61
62 pprz_msg_send_GVF(
63 trans, dev, AC_ID,
64 &gvf_ik_control.phi,
65 &traj_type,
67 &gvf_ik_control.ke,
69 &gvf_ik_control.error_n,
70 &gvf_ik_telemetry.n_norm,
71 &gvf_ik_telemetry.t_norm,
72 &gvf_ik_telemetry.omega_d,
73 &gvf_ik_telemetry.omega);
74
75#if GVF_OCAML_GCS
76 if (gvf_trajectory.type == ELLIPSE &&
77 ((int)gvf_trajectory.p[2] == (int)gvf_trajectory.p[3])) {
78 pprz_msg_send_CIRCLE(trans, dev, AC_ID,
79 &gvf_trajectory.p[0], &gvf_trajectory.p[1],
80 &gvf_trajectory.p[2]);
81 }
82
83 if (gvf_trajectory.type == LINE && gvf_segment.seg == 1) {
84 pprz_msg_send_SEGMENT(trans, dev, AC_ID,
85 &gvf_segment.x1, &gvf_segment.y1,
86 &gvf_segment.x2, &gvf_segment.y2);
87 }
88#endif // GVF_OCAML_GCS
89
90 }
91}
92
93#endif // PERIODIC_TELEMETRY
94
96
98
99static void reset_gamma_t0(void)
100{
102}
103
104static bool check_alpha(float ke, float J1, float J2, float phi, float speed)
105{
106 float J_Jt = (J1*J1 + J2*J2);
107
108 float e = phi - gvf_ik_control.gamma;
109 float e_tdot = - gvf_ik_control.gamma_dot;
110
111 float u = - ke * e;
112
113 float nx = J1 / J_Jt * (u - e_tdot);
114 float ny = J2 / J_Jt * (u - e_tdot);
115
116 float un_norm2 = nx*nx + ny*ny;
117
118 return un_norm2 < speed*speed;
119}
120
122
123void gvf_ik_init(void)
124{
125 gvf_ik_control.ke = 1;
126 gvf_ik_control.kn = 1;
127 gvf_ik_control.s = 1;
128 gvf_trajectory.type = NONE;
129
130 gvf_ik_control.gamma_amplitude = GVF_IK_GAMMA_AMPLITUDE;
132
134
135#if PERIODIC_TELEMETRY
137#endif // PERIODIC_TELEMETRY
138}
139
140// GENERIC TRAJECTORY CONTROLLER
141void gvf_ik_control_2D(float ke, float kn, float phi,
142 struct gvf_grad *grad, struct gvf_Hess *hess)
143{
144
145 // compute gamma period
146 float gamma_period = 2 * M_PI / gvf_ik_control.gamma_omega;
147
148 // compute gamma t
150 float t = (last_gvf_t0 - gamma_t0) / 1000;
151
152 if (t > gamma_period) {
154 }
155
156 // get state
158 float course = gvf_c_state.course;
159 float px_dot = gvf_c_state.px_dot;
160 float py_dot = gvf_c_state.py_dot;
161
162 float speed = sqrtf(px_dot*px_dot + py_dot*py_dot);
163 float speed2 = speed * speed;
164
165 int s = gvf_ik_control.s;
166
167 // gradient Phi
168 float J1 = grad->nx;
169 float J2 = grad->ny;
170 float J_Jt = (J1*J1 + J2*J2);
171
172 // Hessian
173 float H11 = hess->H11;
174 float H12 = hess->H12;
175 float H21 = hess->H21;
176 float H22 = hess->H22;
177
178 // -- Calculation of the desired angular velocity in the vector field --------
179 gvf_ik_control.gamma = gvf_ik_control.gamma_amplitude * sinf(gvf_ik_control.gamma_omega * t);
180 gvf_ik_control.gamma_dot = gvf_ik_control.gamma_omega * gvf_ik_control.gamma_amplitude * cosf(gvf_ik_control.gamma_omega * t);
181
182 bool cond_flag = check_alpha(ke, J1, J2, phi, speed);
183
184 float e = phi;
185 float e_tdot = 0;
186 float e_tddot = 0;
187 if (cond_flag) {
188 e = e - gvf_ik_control.gamma;
189 e_tdot = - gvf_ik_control.gamma_dot;
190 e_tddot = (gvf_ik_control.gamma_omega * gvf_ik_control.gamma_omega) * gvf_ik_control.gamma;
191 }
192
193 // control law
194 float u = - ke * e;
195
196 // normal to Phi in p_dot (IK)
197 float nx = J1 / J_Jt * (u - e_tdot);
198 float ny = J2 / J_Jt * (u - e_tdot);
199
200 float n_norm2 = nx*nx + ny*ny;
201 float n_norm = sqrtf(n_norm2);
202
203 // tangent to Phi in p_dot
204 float tx = s * J2;
205 float ty = -s * J1;
206
207 float t_norm = sqrtf(tx*tx + ty*ty);
208 float tx_hat = tx / t_norm;
209 float ty_hat = ty / t_norm;
210
211 // compute alpha and p_dot
212 float alpha = 0;
213 float pdx_dot = 0;
214 float pdy_dot = 0;
215
216 if (cond_flag){
217 alpha = sqrtf(speed2 - n_norm2);
218 pdx_dot = alpha * tx_hat + nx;
219 pdy_dot = alpha * ty_hat + ny;
220 } else {
221 pdx_dot = speed * nx / n_norm;
222 pdy_dot = speed * ny / n_norm;
223 }
224
225 // compute n_dot
226 float u_dot = - ke * (J1*pdx_dot + J2*pdy_dot + e_tdot);
227
228 float un_dot_A_x = (pdx_dot*H11 + pdy_dot*H21);
229 float un_dot_A_y = (pdx_dot*H12 + pdy_dot*H22);
230
231 float B_term = 2 * ((H11*pdx_dot + H21*pdy_dot)*J1 + (H21*pdx_dot + H22*pdy_dot)*J2) / J_Jt;
232 float un_dot_B_x = - J1 * B_term;
233 float un_dot_B_y = - J2 * B_term;
234
235 float C_term = (u_dot - e_tddot) / J_Jt;
236 float un_dot_C_x = J1 * C_term;
237 float un_dot_C_y = J2 * C_term;
238
239 float nx_dot = (un_dot_A_x + un_dot_B_x) * (u - e_tdot) / J_Jt + un_dot_C_x;
240 float ny_dot = (un_dot_A_y + un_dot_B_y) * (u - e_tdot) / J_Jt + un_dot_C_y;
241
242 // compute omega_d and omega
243 float pd_ddot_x = 0;
244 float pd_ddot_y = 0;
245
246 if (cond_flag){
247 float alpha_dot = - (nx*nx_dot + ny*ny_dot) / (alpha);
248
249 float tx_dot = s * (H12 * pdx_dot + H22 * pdy_dot);
250 float ty_dot = - s * (H11 * pdx_dot + H21 * pdy_dot);
251
252 float t_norm3 = t_norm * t_norm * t_norm;
253 float Bpd_ddot_x = alpha * (tx_dot / t_norm + (tx*tx*tx_dot - tx*ty*ty_dot) / t_norm3);
254 float Bpd_ddot_y = alpha * (ty_dot / t_norm + (tx*ty*tx_dot - ty*ty*ty_dot) / t_norm3);
255
256 pd_ddot_x = alpha_dot * tx_hat + Bpd_ddot_x + nx_dot;
257 pd_ddot_y = alpha_dot * ty_hat + Bpd_ddot_y + ny_dot;
258 } else {
259 float n_norm3 = n_norm2 * n_norm;
260 pd_ddot_x = speed * (nx_dot / n_norm + (nx*nx*nx_dot - nx*ny*ny_dot) / n_norm3);
261 pd_ddot_y = speed * (ny_dot / n_norm + (nx*ny*nx_dot - ny*ny*ny_dot) / n_norm3);
262 }
263
264
265 float omega_d = - (- pdx_dot*pd_ddot_y + pdy_dot*pd_ddot_x) / speed2;
266
267 float rx = speed * sinf(course);
268 float ry = speed * cosf(course);
269
270 float e_n = (pdx_dot*ry - pdy_dot*rx) / speed2;
271 float omega = omega_d - kn * e_n;
272
273 // Update external control-related and telemetry variables
274 gvf_ik_control.phi = phi;
275 gvf_ik_control.error = e;
276 gvf_ik_control.error_n = e_n;
277 gvf_ik_telemetry.n_norm = n_norm;
278 gvf_ik_telemetry.t_norm = alpha;
279 gvf_ik_telemetry.omega_d = omega_d;
280 gvf_ik_telemetry.omega = omega;
281
282 //printf("t: %f, pdx_dot: %f, pdy_dot: %f, rx: %f , ry: %f, omega: %f \n", t, pdx_dot, pdy_dot, rx, ry, omega);
283
284 // ---------------------------------------------------------------------------
285
286 // Set GVF common info
287 gvf_c_info.kappa = (J1*(H12*J2 - J1*H22) + J2*(H21*J1 - H11*J2))/powf(J_Jt, 1.5);
288 gvf_c_info.ori_err = e_n;
289
290 // Send the computed omega to the low level control
292}
293
298
static int16_t course[3]
Core autopilot interface common to all firmwares.
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
static struct uart_periph * dev
static void send_gvf(struct transport_tx *trans, struct link_device *dev)
TELEMETRY -----------------------------------------------------------—.
Definition gvf.c:42
void gvf_low_level_control_2D(float omega)
Definition gvf_common.c:58
gvf_common_info gvf_c_info
Definition gvf_common.c:29
gvf_common_state gvf_c_state
Definition gvf_common.c:33
void gvf_low_level_getState(void)
Definition gvf_common.c:37
void gvf_ik_control_2D(float ke, float kn, float phi, struct gvf_grad *grad, struct gvf_Hess *hess)
Definition gvf_ik.c:141
static void reset_gamma_t0(void)
STATIC FUNCTIONS ----------------------------------------------------—.
Definition gvf_ik.c:99
static void send_gvf(struct transport_tx *trans, struct link_device *dev)
TELEMETRY -----------------------------------------------------------—.
Definition gvf_ik.c:46
void gvf_ik_init(void)
EXTERN FUNCTIONS ----------------------------------------------------—.
Definition gvf_ik.c:123
static uint32_t last_gvf_t0
Definition gvf_ik.c:40
gvf_ik_con gvf_ik_control
Definition gvf_ik.c:35
static uint32_t gamma_t0
Definition gvf_ik.c:39
void gvf_ik_set_direction(int8_t s)
Definition gvf_ik.c:294
static bool check_alpha(float ke, float J1, float J2, float phi, float speed)
Definition gvf_ik.c:104
gvf_ik_tel gvf_ik_telemetry
Definition gvf_ik.c:36
#define GVF_IK_GAMMA_OMEGA
Definition gvf_ik.h:39
#define GVF_IK_GAMMA_AMPLITUDE
Definition gvf_ik.h:35
gvf_seg gvf_segment
Definition gvf_traj.c:25
gvf_tra gvf_trajectory
Definition gvf_traj.c:24
float H12
Definition gvf_traj.h:47
float H21
Definition gvf_traj.h:49
float H11
Definition gvf_traj.h:46
@ ELLIPSE
Definition gvf_traj.h:28
@ LINE
Definition gvf_traj.h:27
float ny
Definition gvf_traj.h:41
float nx
Definition gvf_traj.h:40
float H22
Definition gvf_traj.h:50
static uint32_t s
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
float alpha
Definition textons.c:133
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
signed char int8_t
Typedef defining 8 bit char type.