Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
gvf.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.h"
27#include "autopilot.h"
28
29// Control
31
32// Telemetry
34
35// Time variables to check if GVF is active
36static uint32_t gvf_t0 = 0;
37
40#if PERIODIC_TELEMETRY
42static void send_gvf(struct transport_tx *trans, struct link_device *dev)
43{
45
47 uint32_t delta_T = now - gvf_t0;
48
49 if (delta_T < 200) {
51 trans, dev, AC_ID,
53 &traj_type,
54 &gvf_control.s,
62
63#if GVF_OCAML_GCS
65 ((int)gvf_trajectory.p[2] == (int)gvf_trajectory.p[3])) {
68 &gvf_trajectory.p[2]);
69 }
70
71 if (gvf_trajectory.type == LINE && gvf_segment.seg == 1) {
75 }
76#endif // GVF_OCAML_GCS
77
78 }
79}
80
81#endif // PERIODIC_TELEMETRY
82
85void gvf_init(void)
86{
87 gvf_control.ke = 1;
88 gvf_control.kn = 1;
89 gvf_control.s = 1;
90 gvf_control.speed = 1; // Rotorcraft only (for now)
91 gvf_control.align = false; // Rotorcraft only
93
94#if PERIODIC_TELEMETRY
96#endif
97}
98
99// GENERIC TRAJECTORY CONTROLLER
100void gvf_control_2D(float ke, float kn __attribute__((unused)), float e,
101 struct gvf_grad *grad, struct gvf_Hess *hess)
102{
104
106 float course __attribute__((unused)) = gvf_c_state.course;
107 float px_dot __attribute__((unused)) = gvf_c_state.px_dot;
108 float py_dot __attribute__((unused)) = gvf_c_state.py_dot;
109
110 int s = gvf_control.s;
111
112 // gradient Phi
113 float nx = grad->nx;
114 float ny = grad->ny;
115
116 // tangent to Phi
117 float tx = s * grad->ny;
118 float ty = -s * grad->nx;
119
120 // Hessian
121 float H11 __attribute__((unused)) = hess->H11;
122 float H12 __attribute__((unused)) = hess->H12;
123 float H21 __attribute__((unused)) = hess->H21;
124 float H22 __attribute__((unused)) = hess->H22;
125
126 // Calculation of the desired angular velocity in the vector field
127 float pdx_dot = tx - ke * e * nx;
128 float pdy_dot = ty - ke * e * ny;
129
131 float md_x = pdx_dot / norm_pd_dot;
132 float md_y = pdy_dot / norm_pd_dot;
133
134 #if defined(ROTORCRAFT_FIRMWARE) // TODO: new accel GVF module just for rotorcrafts
135
136 (void)(course);
137
138 // Use accel based control. Not recommended as of current implementation
139 #if defined(GVF_ROTORCRAFT_USE_ACCEL)
140
141 // Set nav for command
142 // Use parameter kn as the speed command
143 nav.speed.x = md_x * kn;
144 nav.speed.y = md_y * kn;
145
146 // Acceleration induced by the field with speed set to kn (!WIP!)
147 #warning "Using GVF for rotorcraft is still experimental, proceed with caution"
148 float n_norm = sqrtf(nx*nx+ny*ny);
149 float hess_px_dot = px_dot * H11 + py_dot * H12;
150 float hess_py_dot = px_dot * H21 + py_dot * H22;
151
152 float hess_pdx_dot = pdx_dot * H11 + pdy_dot * H12;
153 float hess_pdy_dot = pdx_dot * H21 + pdy_dot * H22;
154
155 float curvature_correction = tx * hess_px_dot + ty * hess_py_dot / (n_norm * n_norm);
156 float accel_correction_x = kn * hess_py_dot / n_norm;
157 float accel_correction_y = - kn * hess_px_dot / n_norm;
160
161 float speed_cmd_x = kn*tx / n_norm - ke * e * nx / (n_norm);
162 float speed_cmd_y = kn*ty / n_norm - ke * e * ny / (n_norm);
163
164 // TODO: don't change nav struct directly
165 nav.accel.x = accel_cmd_x + (speed_cmd_x - px_dot);
166 nav.accel.y = accel_cmd_y + (speed_cmd_y - py_dot);
168
169 #else // SPEED_BASED_GVF // TODO: move to low level control??
170
172
173 // Speed-based control, acceleration based control not implemented yet
176
177 // Optionally align heading with trajectory
178 if (gvf_control.align)
179 {
181 }
182
183 #endif
184
185 #else // FIXEDWING & ROVER FIRMWARE
186
187 float Apd_dot_dot_x = -ke * (nx * px_dot + ny * py_dot) * nx;
188 float Apd_dot_dot_y = -ke * (nx * px_dot + ny * py_dot) * ny;
189
190 float Bpd_dot_dot_x = ((-ke * e * H11) + s * H21) * px_dot
191 + ((-ke * e * H12) + s * H22) * py_dot;
192 float Bpd_dot_dot_y = -(s * H11 + (ke * e * H21)) * px_dot
193 - (s * H12 + (ke * e * H22)) * py_dot;
194
197
199 / norm_pd_dot;
200
201 float md_dot_x = md_y * md_dot_const;
202 float md_dot_y = -md_x * md_dot_const;
203
204 float omega_d = -(md_dot_x * md_y - md_dot_y * md_x);
205
206 float mr_x = sinf(course);
207 float mr_y = cosf(course);
208
209 float e_n = (mr_x * md_y - mr_y * md_x);
210 float omega = omega_d + kn * e_n;
211
213
214 // ------------------------------------------------
215
216 // Telemetry data
217 gvf_telemetry.n_norm = ke*e*sqrtf(nx*nx + ny*ny);
219 gvf_telemetry.omega_d = omega_d;
220 gvf_telemetry.omega = omega;
221
222 // Set GVF common info
223 gvf_c_info.kappa = (nx*(H12*ny - nx*H22) + ny*(H21*nx - H11*ny))/powf(nx*nx + ny*ny,1.5);
225
226 // Call the low level control
228
229 #endif
230}
231
233{
234 gvf_control.s = s;
235}
236
237// BEGIN ROTORCRAFT
238
239void gvf_set_speed(float speed)
240{
241 if (speed < 0.0) speed = 0.0;
242 gvf_control.speed = speed;
243}
244
245void gvf_set_align(bool align)
246{
247 gvf_control.align = align;
248}
249
250// END ROTORCRAFT
251
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.
void gvf_set_align(bool align)
Definition gvf.c:245
gvf_con gvf_control
Definition gvf.c:30
void gvf_init(void)
EXTERN FUNCTIONS ----------------------------------------------------—.
Definition gvf.c:85
void gvf_control_2D(float ke, float kn, float e, struct gvf_grad *grad, struct gvf_Hess *hess)
Definition gvf.c:100
static void send_gvf(struct transport_tx *trans, struct link_device *dev)
TELEMETRY -----------------------------------------------------------—.
Definition gvf.c:42
void gvf_set_speed(float speed)
Definition gvf.c:239
void gvf_set_direction(int8_t s)
Definition gvf.c:232
gvf_tel gvf_telemetry
Definition gvf.c:33
static uint32_t gvf_t0
Definition gvf.c:36
Guidance algorithm based on vector fields.
float ke
Definition gvf.h:54
float error
Definition gvf.h:56
float error_n
Definition gvf.h:57
float speed
Definition gvf.h:58
float omega
Definition gvf.h:67
int8_t s
Definition gvf.h:59
float omega_d
Definition gvf.h:66
float n_norm
Definition gvf.h:64
bool align
Definition gvf.h:60
float kn
Definition gvf.h:55
float t_norm
Definition gvf.h:65
Definition gvf.h:53
Definition gvf.h:63
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
gvf_seg gvf_segment
Definition gvf_traj.c:25
gvf_tra gvf_trajectory
Definition gvf_traj.c:24
float x1
Definition gvf_traj.h:68
enum trajectories type
Definition gvf_traj.h:34
float p[16]
Definition gvf_traj.h:35
int p_len
Definition gvf_traj.h:36
@ ELLIPSE
Definition gvf_traj.h:28
@ LINE
Definition gvf_traj.h:27
int seg
Definition gvf_traj.h:67
float y1
Definition gvf_traj.h:69
float y2
Definition gvf_traj.h:71
float x2
Definition gvf_traj.h:70
static uint32_t s
uint16_t foo
Definition main_demo5.c:58
float y
in meters
float x
in meters
struct RotorcraftNavigation nav
Definition navigation.c:51
struct EnuCoor_f speed
speed setpoint (in m/s)
Definition navigation.h:128
#define NAV_SETPOINT_MODE_SPEED
Definition navigation.h:101
struct EnuCoor_f accel
accel setpoint (in m/s)
Definition navigation.h:129
float heading
heading setpoint (in radians)
Definition navigation.h:133
static const struct usb_device_descriptor dev
Definition usb_ser_hw.c:74
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_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
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.