diff --git a/conf/flight_plans/demo_gvf.xml b/conf/flight_plans/demo_gvf.xml
new file mode 100644
index 0000000000..b18f37e90d
--- /dev/null
+++ b/conf/flight_plans/demo_gvf.xml
@@ -0,0 +1,78 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/gvf_module.xml b/conf/modules/gvf_module.xml
new file mode 100644
index 0000000000..8898f854bf
--- /dev/null
+++ b/conf/modules/gvf_module.xml
@@ -0,0 +1,72 @@
+
+
+
+
+ Guidance algorithm for tracking smooth trajectories. The algorithm is based on the idea of stearing the vehicle to a vector field that smoothly converges to the desired trajectory.
+For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_vector_field .
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/telemetry/default_fixedwing_gvf.xml b/conf/telemetry/default_fixedwing_gvf.xml
new file mode 100644
index 0000000000..7c3b2b9629
--- /dev/null
+++ b/conf/telemetry/default_fixedwing_gvf.xml
@@ -0,0 +1,93 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/modules/guidance/gvf/gvf.c b/sw/airborne/modules/guidance/gvf/gvf.c
new file mode 100644
index 0000000000..4f1be41a95
--- /dev/null
+++ b/sw/airborne/modules/guidance/gvf/gvf.c
@@ -0,0 +1,301 @@
+/*
+ * Copyright (C) 2016 Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+#include
+#include "std.h"
+
+#include "modules/guidance/gvf/gvf.h"
+#include "modules/guidance/gvf/trajectories/gvf_ellipse.h"
+#include "modules/guidance/gvf/trajectories/gvf_line.h"
+#include "modules/guidance/gvf/trajectories/gvf_sin.h"
+
+#include "subsystems/navigation/common_nav.h"
+#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
+#include "firmwares/fixedwing/autopilot.h"
+
+// Control
+gvf_con gvf_control;
+
+// Trajectory
+gvf_tra gvf_trajectory;
+
+#if PERIODIC_TELEMETRY
+#include "subsystems/datalink/telemetry.h"
+static void send_gvf(struct transport_tx *trans, struct link_device *dev)
+{
+ // Do not know whether is a good idea to do this check here or to include
+ // this plen in gvf_trajectory
+ int plen;
+
+ switch (gvf_trajectory.type) {
+ case LINE:
+ plen = 3;
+ break;
+ case ELLIPSE:
+ plen = 5;
+ break;
+ case SIN:
+ plen = 6;
+ break;
+ default:
+ plen = 0;
+ break;
+ }
+
+ uint8_t traj_type = (uint8_t)gvf_trajectory.type;
+
+ pprz_msg_send_GVF(trans, dev, AC_ID, &gvf_control.error, &traj_type,
+ &gvf_control.s, plen, gvf_trajectory.p);
+}
+
+static void send_circle(struct transport_tx *trans, struct link_device *dev)
+{
+ if (gvf_trajectory.type == ELLIPSE &&
+ (gvf_trajectory.p[2] == gvf_trajectory.p[3])) {
+ pprz_msg_send_CIRCLE(trans, dev, AC_ID,
+ &gvf_trajectory.p[0], &gvf_trajectory.p[1],
+ &gvf_trajectory.p[2]);
+ }
+}
+
+#endif
+
+void gvf_init(void)
+{
+ gvf_control.ke = 1;
+ gvf_control.kn = 1;
+ gvf_control.s = 1;
+ gvf_trajectory.type = NONE;
+
+#if PERIODIC_TELEMETRY
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GVF, send_gvf);
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CIRCLE,
+ send_circle);
+#endif
+}
+
+// GENERIC TRAJECTORY CONTROLLER
+void gvf_control_2D(float ke, float kn, float e,
+ struct gvf_grad *grad, struct gvf_Hess *hess)
+{
+ struct FloatEulers *att = stateGetNedToBodyEulers_f();
+ float ground_speed = stateGetHorizontalSpeedNorm_f();
+ float course = stateGetHorizontalSpeedDir_f();
+ float px_dot = ground_speed * sinf(course);
+ float py_dot = ground_speed * cosf(course);
+ int s = gvf_control.s;
+
+ // gradient Phi
+ float nx = grad->nx;
+ float ny = grad->ny;
+
+ // tangent to Phi
+ float tx = s * grad->ny;
+ float ty = -s * grad->nx;
+
+ // Hessian
+ float H11 = hess->H11;
+ float H12 = hess->H12;
+ float H21 = hess->H21;
+ float H22 = hess->H22;
+
+ // Calculation of the desired angular velocity in the vector field
+ float pdx_dot = tx - ke * e * nx;
+ float pdy_dot = ty - ke * e * ny;
+
+ float norm_pd_dot = sqrtf(pdx_dot * pdx_dot + pdy_dot * pdy_dot);
+ float md_x = pdx_dot / norm_pd_dot;
+ float md_y = pdy_dot / norm_pd_dot;
+
+ float Apd_dot_dot_x = -ke * (nx * px_dot + ny * py_dot) * nx;
+ float Apd_dot_dot_y = -ke * (nx * px_dot + ny * py_dot) * ny;
+
+ float Bpd_dot_dot_x = ((-ke * e * H11) + s * H21) * px_dot
+ + ((-ke * e * H12) + s * H22) * py_dot;
+ float Bpd_dot_dot_y = -(s * H11 + (ke * e * H21)) * px_dot
+ - (s * H12 + (ke * e * H22)) * py_dot;
+
+ float pd_dot_dot_x = Apd_dot_dot_x + Bpd_dot_dot_x;
+ float pd_dot_dot_y = Apd_dot_dot_y + Bpd_dot_dot_y;
+
+ float md_dot_const = -(md_x * pd_dot_dot_y - md_y * pd_dot_dot_x)
+ / norm_pd_dot;
+
+ float md_dot_x = md_y * md_dot_const;
+ float md_dot_y = -md_x * md_dot_const;
+
+ float omega_d = -(md_dot_x * md_y - md_dot_y * md_x);
+
+ float mr_x = sinf(course);
+ float mr_y = cosf(course);
+
+ float omega = omega_d + kn * (mr_x * md_y - mr_y * md_x);
+
+ // Coordinated turn
+ h_ctl_roll_setpoint =
+ -atanf(omega * ground_speed / GVF_GRAVITY / cosf(att->theta));
+ BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
+
+ lateral_mode = LATERAL_MODE_ROLL;
+}
+
+void gvf_set_gains(float ke, float kn)
+{
+ gvf_control.ke = ke;
+ gvf_control.kn = kn;
+}
+
+void gvf_set_direction(int8_t s)
+{
+ gvf_control.s = s;
+}
+
+// STRAIGHT LINE
+
+void gvf_line(float a, float b, float alpha)
+{
+ float e;
+ struct gvf_grad grad_line;
+ struct gvf_Hess Hess_line;
+
+ gvf_trajectory.type = 0;
+ gvf_trajectory.p[0] = a;
+ gvf_trajectory.p[1] = b;
+ gvf_trajectory.p[2] = alpha;
+
+ gvf_line_info(&e, &grad_line, &Hess_line);
+ gvf_control_2D(1e-2 * gvf_control.ke, gvf_control.kn, e, &grad_line, &Hess_line);
+
+ gvf_control.error = e;
+}
+
+bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2)
+{
+ float x1 = waypoints[wp1].x;
+ float y1 = waypoints[wp1].y;
+ float x2 = waypoints[wp2].x;
+ float y2 = waypoints[wp2].y;
+
+ float zx = x1 - x2;
+ float zy = y1 - y2;
+
+ float alpha = atanf(zy / zx);
+
+ gvf_line(x1, y1, alpha);
+
+ return true;
+}
+
+bool gvf_line_wp_heading(uint8_t wp, float alpha)
+{
+ alpha = alpha * M_PI / 180;
+
+ float a = waypoints[wp].x;
+ float b = waypoints[wp].y;
+
+ gvf_line(a, b, alpha);
+
+ return true;
+}
+
+// ELLIPSE
+bool gvf_ellipse(uint8_t wp, float a, float b, float alpha)
+{
+ float e;
+ struct gvf_grad grad_ellipse;
+ struct gvf_Hess Hess_ellipse;
+
+ gvf_trajectory.type = 1;
+ gvf_trajectory.p[0] = waypoints[wp].x;
+ gvf_trajectory.p[1] = waypoints[wp].y;
+ gvf_trajectory.p[2] = a;
+ gvf_trajectory.p[3] = b;
+ gvf_trajectory.p[4] = alpha;
+
+ // SAFE MODE
+ if (a < 1 || b < 1) {
+ gvf_trajectory.p[2] = 60;
+ gvf_trajectory.p[3] = 60;
+ }
+
+ gvf_ellipse_info(&e, &grad_ellipse, &Hess_ellipse);
+ gvf_control_2D(gvf_control.ke, gvf_control.kn, e, &grad_ellipse, &Hess_ellipse);
+
+ gvf_control.error = e;
+
+ return true;
+}
+
+// SINUSOIDAL (if w = 0 and off = 0, then we just have the straight line case)
+
+void gvf_sin(float a, float b, float alpha, float w, float off, float A)
+{
+ float e;
+ struct gvf_grad grad_line;
+ struct gvf_Hess Hess_line;
+
+ gvf_trajectory.type = 2;
+ gvf_trajectory.p[0] = a;
+ gvf_trajectory.p[1] = b;
+ gvf_trajectory.p[2] = alpha;
+ gvf_trajectory.p[3] = w;
+ gvf_trajectory.p[4] = off;
+ gvf_trajectory.p[5] = A;
+
+ gvf_sin_info(&e, &grad_line, &Hess_line);
+ gvf_control_2D(1e-2 * gvf_control.ke, gvf_control.kn, e, &grad_line, &Hess_line);
+
+ gvf_control.error = e;
+}
+
+bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A)
+{
+ w = 2 * M_PI * w;
+
+ float x1 = waypoints[wp1].x;
+ float y1 = waypoints[wp1].y;
+ float x2 = waypoints[wp2].x;
+ float y2 = waypoints[wp2].y;
+
+ float zx = x1 - x2;
+ float zy = y1 - y2;
+
+ float alpha = atanf(zy / zx);
+
+ gvf_sin(x1, y1, alpha, w, off, A);
+
+ return true;
+}
+
+bool gvf_sin_wp_heading(uint8_t wp, float alpha, float w, float off, float A)
+{
+ w = 2 * M_PI * w;
+ alpha = alpha * M_PI / 180;
+
+ float x = waypoints[wp].x;
+ float y = waypoints[wp].y;
+
+ gvf_sin(x, y, alpha, w, off, A);
+
+ return true;
+}
+
diff --git a/sw/airborne/modules/guidance/gvf/gvf.h b/sw/airborne/modules/guidance/gvf/gvf.h
new file mode 100644
index 0000000000..94d0cbec8b
--- /dev/null
+++ b/sw/airborne/modules/guidance/gvf/gvf.h
@@ -0,0 +1,98 @@
+/*
+ * Copyright (C) 2016 Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file gvf.h
+ *
+ * Guidance algorithm based on vector fields
+ */
+
+#ifndef GVF_H
+#define GVF_H
+
+#define GVF_GRAVITY 9.806
+
+#include "std.h"
+
+typedef struct {
+ float error;
+ float ke;
+ float kn;
+ int8_t s;
+} gvf_con;
+
+extern gvf_con gvf_control;
+
+enum trajectories {
+ LINE = 0,
+ ELLIPSE,
+ SIN,
+ NONE = 255,
+};
+
+typedef struct {
+ enum trajectories type;
+ float p[16];
+} gvf_tra;
+
+extern gvf_tra gvf_trajectory;
+
+struct gvf_grad {
+ float nx;
+ float ny;
+ float nz;
+};
+
+struct gvf_Hess {
+ float H11;
+ float H12;
+ float H13;
+ float H21;
+ float H22;
+ float H23;
+ float H31;
+ float H32;
+ float H33;
+};
+
+extern void gvf_init(void);
+void gvf_control_2D(float ke, float kn, float e,
+ struct gvf_grad *, struct gvf_Hess *);
+extern void gvf_set_gains(float ke, float kd);
+extern void gvf_set_direction(int8_t s);
+
+// Straigh line
+void gvf_line(float x, float y, float alpha);
+extern bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2);
+extern bool gvf_line_wp_heading(uint8_t wp, float alpha);
+
+// Ellipse
+extern bool gvf_ellipse(uint8_t wp, float a, float b, float alpha);
+
+// Sinusoidal
+void gvf_sin(float x, float y, float alpha, float w, float off, float A);
+extern bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off,
+ float A);
+extern bool gvf_sin_wp_heading(uint8_t wp, float alpha, float w, float off,
+ float A);
+
+
+#endif // GVF_H
diff --git a/sw/airborne/modules/guidance/gvf/trajectories/gvf_ellipse.c b/sw/airborne/modules/guidance/gvf/trajectories/gvf_ellipse.c
new file mode 100644
index 0000000000..ba9b6aff2f
--- /dev/null
+++ b/sw/airborne/modules/guidance/gvf/trajectories/gvf_ellipse.c
@@ -0,0 +1,79 @@
+/*
+ * Copyright (C) 2016 Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file gvf_ellipse.c
+ *
+ * Guidance algorithm based on vector fields
+ * 2D Ellipse trajectory
+ */
+
+
+#include "subsystems/navigation/common_nav.h"
+#include "gvf_ellipse.h"
+
+#ifndef GVF_ELLIPSE_A
+#define GVF_ELLIPSE_A 80
+#endif
+
+#ifndef GVF_ELLIPSE_B
+#define GVF_ELLIPSE_B 80
+#endif
+
+#ifndef GVF_ELLIPSE_ALPHA
+#define GVF_ELLIPSE_ALPHA 0
+#endif
+
+gvf_ell_par gvf_ellipse_par = {GVF_ELLIPSE_A, GVF_ELLIPSE_B, GVF_ELLIPSE_ALPHA};
+
+void gvf_ellipse_info(float *phi, struct gvf_grad *grad,
+ struct gvf_Hess *hess)
+{
+
+ struct EnuCoor_f *p = stateGetPositionEnu_f();
+ float px = p->x;
+ float py = p->y;
+ float wx = gvf_trajectory.p[0];
+ float wy = gvf_trajectory.p[1];
+ float a = gvf_trajectory.p[2];
+ float b = gvf_trajectory.p[3];
+ float alpha = gvf_trajectory.p[4];
+
+ float cosa = cosf(alpha);
+ float sina = sinf(alpha);
+
+ // Phi(x,y)
+ float xel = (px - wx) * cosa - (py - wy) * sina;
+ float yel = (px - wx) * sina + (py - wy) * cosa;
+ *phi = (xel / a) * (xel / a) + (yel / b) * (yel / b) - 1;
+
+ // grad Phi
+ grad->nx = (2 * xel / (a * a)) * cosa + (2 * yel / (b * b)) * sina;
+ grad->ny = (2 * yel / (b * b)) * cosa - (2 * xel / (a * a)) * sina;
+
+ // Hessian Phi
+ hess->H11 = 2 * (cosa * cosa / (a * a)
+ + sina * sina / (b * b));
+ hess->H12 = 2 * sina * cosa * (1 / (b * b) - 1 / (a * a));
+ hess->H21 = hess->H12;
+ hess->H22 = 2 * (sina * sina / (a * a)
+ + cosa * cosa / (b * b));
+}
diff --git a/sw/airborne/modules/guidance/gvf/trajectories/gvf_ellipse.h b/sw/airborne/modules/guidance/gvf/trajectories/gvf_ellipse.h
new file mode 100644
index 0000000000..3118a30f6b
--- /dev/null
+++ b/sw/airborne/modules/guidance/gvf/trajectories/gvf_ellipse.h
@@ -0,0 +1,44 @@
+/*
+ * Copyright (C) 2016 Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file gvf_ellipse.h
+ *
+ * Guidance algorithm based on vector fields
+ * 2D Ellipse trajectory
+ */
+
+#ifndef GVF_ELLIPSE_H
+#define GVF_ELLIPSE_H
+
+#include "modules/guidance/gvf/gvf.h"
+
+typedef struct {
+ float a;
+ float b;
+ float alpha;
+} gvf_ell_par;
+
+extern gvf_ell_par gvf_ellipse_par;
+
+extern void gvf_ellipse_info(float *phi, struct gvf_grad *, struct gvf_Hess *);
+
+#endif // GVF_ELLIPSE_H
diff --git a/sw/airborne/modules/guidance/gvf/trajectories/gvf_line.c b/sw/airborne/modules/guidance/gvf/trajectories/gvf_line.c
new file mode 100644
index 0000000000..d7ee8ba149
--- /dev/null
+++ b/sw/airborne/modules/guidance/gvf/trajectories/gvf_line.c
@@ -0,0 +1,62 @@
+/*
+ * Copyright (C) 2016 Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file gvf_line.c
+ *
+ * Guidance algorithm based on vector fields
+ * 2D straight line trajectory
+ */
+
+
+#include "subsystems/navigation/common_nav.h"
+#include "gvf_line.h"
+
+#ifndef GVF_LINE_ALPHA
+#define GVF_LINE_ALPHA 0
+#endif
+
+gvf_li_par gvf_line_par = {GVF_LINE_ALPHA};
+
+void gvf_line_info(float *phi, struct gvf_grad *grad,
+ struct gvf_Hess *hess)
+{
+
+ struct EnuCoor_f *p = stateGetPositionEnu_f();
+ float px = p->x;
+ float py = p->y;
+ float a = gvf_trajectory.p[0];
+ float b = gvf_trajectory.p[1];
+ float alpha = gvf_trajectory.p[2];
+
+ // Phi(x,y)
+ *phi = -(px - a) * cosf(alpha) + (py - b) * sinf(alpha);
+
+ // grad Phi
+ grad->nx = -cosf(alpha);
+ grad->ny = sinf(alpha);
+
+ // Hessian Phi
+ hess->H11 = 0;
+ hess->H12 = 0;
+ hess->H21 = 0;
+ hess->H22 = 0;
+}
diff --git a/sw/airborne/modules/guidance/gvf/trajectories/gvf_line.h b/sw/airborne/modules/guidance/gvf/trajectories/gvf_line.h
new file mode 100644
index 0000000000..2470de9b99
--- /dev/null
+++ b/sw/airborne/modules/guidance/gvf/trajectories/gvf_line.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) 2016 Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file gvf_line.h
+ *
+ * Guidance algorithm based on vector fields
+ * 2D straight line trajectory
+ */
+
+#ifndef GVF_LINE_H
+#define GVF_LINE_H
+
+#include "modules/guidance/gvf/gvf.h"
+
+typedef struct {
+ float alpha;
+} gvf_li_par;
+
+extern gvf_li_par gvf_line_par;
+
+extern void gvf_line_info(float *phi, struct gvf_grad *, struct gvf_Hess *);
+
+#endif // GVF_LINE_H
diff --git a/sw/airborne/modules/guidance/gvf/trajectories/gvf_sin.c b/sw/airborne/modules/guidance/gvf/trajectories/gvf_sin.c
new file mode 100644
index 0000000000..c4e73dfe06
--- /dev/null
+++ b/sw/airborne/modules/guidance/gvf/trajectories/gvf_sin.c
@@ -0,0 +1,89 @@
+/*
+ * Copyright (C) 2016 Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file gvf_sin.c
+ *
+ * Guidance algorithm based on vector fields
+ * 2D sinusoidal trajectory
+ */
+
+
+#include "subsystems/navigation/common_nav.h"
+#include "gvf_sin.h"
+
+#ifndef GVF_SIN_ALPHA
+#define GVF_SIN_ALPHA 0
+#endif
+
+#ifndef GVF_SIN_W
+#define GVF_SIN_W 0
+#endif
+
+#ifndef GVF_SIN_OFF
+#define GVF_SIN_OFF 0
+#endif
+
+#ifndef GVF_SIN_A
+#define GVF_SIN_A 0
+#endif
+
+gvf_s_par gvf_sin_par = {GVF_SIN_ALPHA, GVF_SIN_W, GVF_SIN_OFF, GVF_SIN_A};
+
+
+void gvf_sin_info(float *phi, struct gvf_grad *grad,
+ struct gvf_Hess *hess)
+{
+
+ struct EnuCoor_f *p = stateGetPositionEnu_f();
+ float px = p->x;
+ float py = p->y;
+ float a = gvf_trajectory.p[0];
+ float b = gvf_trajectory.p[1];
+ float alpha = gvf_trajectory.p[2];
+ float w = gvf_trajectory.p[3];
+ float off = gvf_trajectory.p[4];
+ float A = gvf_trajectory.p[5];
+
+ float cosa = cosf(alpha);
+ float sina = sinf(alpha);
+
+ // Phi(x,y)
+ float xs = (px - a) * sina - (py - b) * cosa;
+ float ys = -(px - a) * cosa - (py - b) * sina;
+
+ // TODO Make it always in (-pi, pi] in an efficient way
+ float ang = (w * xs + off);
+ float cosang = cosf(ang);
+ float sinang = sinf(ang);
+
+ *phi = ys - A * sinang;
+
+ // grad Phi
+ grad->nx = -cosa - A * w * sina * cosang;
+ grad->ny = -sina + A * w * cosa * cosang;
+
+ // Hessian Phi
+ hess->H11 = A * w * w * sina * sina * sinang;
+ hess->H12 = -A * w * w * sina * cosa * sinang;
+ hess->H21 = -A * w * w * cosa * sina * sinang;
+ hess->H22 = A * w * w * cosa * cosa * sinang;
+}
diff --git a/sw/airborne/modules/guidance/gvf/trajectories/gvf_sin.h b/sw/airborne/modules/guidance/gvf/trajectories/gvf_sin.h
new file mode 100644
index 0000000000..a354ce2d4e
--- /dev/null
+++ b/sw/airborne/modules/guidance/gvf/trajectories/gvf_sin.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright (C) 2016 Hector Garcia de Marina
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/** \file gvf_sin.h
+ *
+ * Guidance algorithm based on vector fields
+ * 2D sinusoidal trajectory
+ */
+
+#ifndef GVF_SIN_H
+#define GVF_SIN_H
+
+#include "modules/guidance/gvf/gvf.h"
+
+typedef struct {
+ float alpha;
+ float w;
+ float off;
+ float A;
+} gvf_s_par;
+
+extern gvf_s_par gvf_sin_par;
+
+extern void gvf_sin_info(float *phi, struct gvf_grad *, struct gvf_Hess *);
+
+#endif // GVF_SIN_H
diff --git a/sw/ground_segment/python/gvf/gvfApp.py b/sw/ground_segment/python/gvf/gvfApp.py
new file mode 100644
index 0000000000..e5dfb1f981
--- /dev/null
+++ b/sw/ground_segment/python/gvf/gvfApp.py
@@ -0,0 +1,18 @@
+#!/usr/bin/env python
+
+import wx
+import gvfframe
+
+class MessagesApp(wx.App):
+ def OnInit(self):
+ self.main = gvfframe.GVFFrame()
+ self.main.Show()
+ self.SetTopWindow(self.main)
+ return True
+
+def main():
+ application = MessagesApp(0)
+ application.MainLoop()
+
+if __name__ == '__main__':
+ main()
diff --git a/sw/ground_segment/python/gvf/gvfframe.py b/sw/ground_segment/python/gvf/gvfframe.py
new file mode 100644
index 0000000000..afc85380fa
--- /dev/null
+++ b/sw/ground_segment/python/gvf/gvfframe.py
@@ -0,0 +1,413 @@
+import wx
+import time
+
+from scipy import linalg as la
+from matplotlib.path import Path
+from matplotlib.backends.backend_wxagg import FigureCanvasWxAgg as FigureCanvas
+import matplotlib.pyplot as pl
+import matplotlib.patches as patches
+import numpy as np
+
+import sys
+from os import path, getenv
+PPRZ_SRC = getenv("PAPARAZZI_SRC", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../../')))
+sys.path.append(PPRZ_SRC + "/sw/lib/python")
+sys.path.append(PPRZ_SRC + "/sw/ext/pprzlink/lib/v1.0/python")
+from pprzlink.ivy import IvyMessagesInterface
+from pprzlink.message import PprzMessage
+from settings_xml_parse import PaparazziACSettings
+
+WIDTH = 800
+HEIGHT = 800
+
+class GVFFrame(wx.Frame):
+ def __init__(self, ac_id=3):
+
+ wx.Frame.__init__(self, id=-1, parent=None, \
+ name=u'GVF', size=wx.Size(WIDTH, HEIGHT), \
+ style=wx.DEFAULT_FRAME_STYLE, title=u'Guidance Vector Field')
+
+ # Vehicle variables
+ self.ac_id = ac_id
+ self.course = 0
+ self.yaw = 0
+ self.XY = np.array([0, 0])
+
+ # Desired trajectory
+ self.timer_traj = 0 # We do not update the traj every time we receive a msg
+ self.timer_traj_lim = 7 # (7+1) * 0.25secs
+ self.s = 0
+ self.kn = 0
+ self.ke = 0
+ self.map_gvf = map2d(np.array([0, 0]), 150000)
+ self.traj = None
+
+ # Frame
+ self.canvas = FigureCanvas(self, -1, self.map_gvf.fig)
+ self.Bind(wx.EVT_CLOSE, self.OnClose)
+
+ self.redraw_timer = wx.Timer(self)
+ self.Bind(wx.EVT_TIMER, self.OnRedrawTimer, self.redraw_timer)
+ self.redraw_timer.Start(100)
+
+ # Ivy
+ self.interface = IvyMessagesInterface("GVF")
+ self.interface.subscribe(self.message_recv)
+ settings = PaparazziACSettings(ac_id)
+ self.ke_index = None
+ self.kn_index = None
+ self.indexes_are_good = 0
+ self.list_of_indexes = ['gvf_ke', 'gvf_kn']
+
+ for setting_ in self.list_of_indexes:
+ try:
+ index = settings.name_lookup[setting_].index
+ if setting_ == 'gvf_ke':
+ self.ke_index = index
+ if setting_ == 'gvf_kn':
+ self.kn_index = index
+ self.indexes_are_good = self.indexes_are_good + 1
+ except Exception as e:
+ print(e)
+ print(setting_ + " setting not found, \
+ have you forgotten gvf.xml in your settings?")
+
+ def message_recv(self, ac_id, msg):
+ if msg.name == 'GPS':
+ self.course = int(msg.get_field(3))*np.pi/1800
+
+ if msg.name == 'NAVIGATION':
+ self.XY[0] = float(msg.get_field(2))
+ self.XY[1] = float(msg.get_field(3))
+
+ if msg.name == 'ATTITUDE':
+ self.yaw = float(msg.get_field(1))
+
+ if msg.name == 'DL_VALUE' and \
+ self.indexes_are_good == len(self.list_of_indexes):
+ if int(msg.get_field(0)) == int(self.ke_index):
+ self.ke = float(msg.get_field(1))
+ if self.traj is not None:
+ self.traj.vector_field(self.traj.XYoff, \
+ self.map_gvf.area, self.s, self.kn, self.ke)
+ if int(msg.get_field(0)) == int(self.kn_index):
+ self.kn = float(msg.get_field(1))
+ if self.traj is not None:
+ self.traj.vector_field(self.traj.XYoff, \
+ self.map_gvf.area, self.s, self.kn, self.ke)
+
+ if msg.name == 'GVF':
+ self.gvf_error = float(msg.get_field(0))
+ # Straight line
+ if int(msg.get_field(1)) == 0 \
+ and self.timer_traj == self.timer_traj_lim:
+ self.s = int(msg.get_field(2))
+ param = [float(x) for x in msg.get_field(3).split(',')]
+ a = param[0]
+ b = param[1]
+ c = param[2]
+
+ self.traj = traj_line(np.array([-100,100]), a, b, c)
+ self.traj.vector_field(self.traj.XYoff, self.map_gvf.area, \
+ self.s, self.kn, self.ke)
+
+ # Ellipse
+ if int(msg.get_field(1)) == 1 \
+ and self.timer_traj == self.timer_traj_lim:
+ self.s = int(msg.get_field(2))
+ param = [float(x) for x in msg.get_field(3).split(',')]
+ ex = param[0]
+ ey = param[1]
+ ea = param[2]
+ eb = param[3]
+ ealpha = param[4]
+ self.traj = traj_ellipse(np.array([ex, ey]), ealpha, ea, eb)
+ self.traj.vector_field(self.traj.XYoff, \
+ self.map_gvf.area, self.s, self.kn, self.ke)
+
+ # Sin
+ if int(msg.get_field(1)) == 2 \
+ and self.timer_traj == self.timer_traj_lim:
+ self.s = int(msg.get_field(2))
+ param = [float(x) for x in msg.get_field(3).split(',')]
+ a = param[0]
+ b = param[1]
+ alpha = param[2]
+ w = param[3]
+ off = param[4]
+ A = param[5]
+ self.traj = traj_sin(np.array([-100, 100]), a, b, alpha, \
+ w, off, A)
+ self.traj.vector_field(self.traj.XYoff, \
+ self.map_gvf.area, self.s, self.kn, self.ke)
+
+ self.timer_traj = self.timer_traj + 1
+ if self.timer_traj > self.timer_traj_lim:
+ self.timer_traj = 0
+
+ def draw_gvf(self, XY, yaw, course):
+ if self.traj is not None:
+ self.map_gvf.draw(XY, yaw, course, self.traj)
+
+ def OnClose(self, event):
+ self.interface.shutdown()
+ self.Destroy()
+
+ def OnRedrawTimer(self, event):
+ self.draw_gvf(self.XY, self.yaw, self.course)
+ self.canvas.draw()
+
+class map2d:
+ def __init__(self, XYoff, area):
+ self.XYoff = XYoff
+ self.area = area
+ self.fig, self.ax = pl.subplots()
+ self.ax.set_xlabel('South [m]')
+ self.ax.set_ylabel('West [m]')
+ self.ax.set_title('2D Map')
+ self.ax.annotate('HOME', xy = (0, 0))
+ self.ax.set_xlim(XYoff[0]-0.5*np.sqrt(area), XYoff[0]+0.5*np.sqrt(area))
+ self.ax.set_ylim(XYoff[1]-0.5*np.sqrt(area), XYoff[1]+0.5*np.sqrt(area))
+ self.ax.axis('equal')
+
+ def vehicle_patch(self, XY, yaw):
+ Rot = np.array([[np.cos(yaw), np.sin(yaw)],[-np.sin(yaw), np.cos(yaw)]])
+
+ apex = 45*np.pi/180 # 30 degrees apex angle
+ b = np.sqrt(2*(self.area/2000) / np.sin(apex))
+ a = b*np.sin(apex/2)
+ h = b*np.cos(apex/2)
+
+ z1 = np.array([a/2, -h*0.3])
+ z2 = np.array([-a/2, -h*0.3])
+ z3 = np.array([0, h*0.6])
+
+ z1 = Rot.dot(z1)
+ z2 = Rot.dot(z2)
+ z3 = Rot.dot(z3)
+
+ verts = [(XY[0]+z1[0], XY[1]+z1[1]), \
+ (XY[0]+z2[0], XY[1]+z2[1]), \
+ (XY[0]+z3[0], XY[1]+z3[1]), \
+ (0, 0)]
+
+ codes = [Path.MOVETO, Path.LINETO, Path.LINETO, Path.CLOSEPOLY]
+ path = Path(verts, codes)
+
+ return patches.PathPatch(path, facecolor='red', lw=2)
+
+ def draw(self, XY, yaw, course, traj):
+ self.ax.clear()
+ self.ax.plot(traj.traj_points[0, :], traj.traj_points[1, :])
+ self.ax.quiver(traj.mapgrad_X, traj.mapgrad_Y, \
+ traj.mapgrad_U, traj.mapgrad_V, color='Teal', \
+ pivot='mid', width=0.002)
+ self.ax.add_patch(self.vehicle_patch(XY, yaw)) # In radians
+ apex = 45*np.pi/180 # 30 degrees apex angle
+ b = np.sqrt(2*(self.area/2000) / np.sin(apex))
+ h = b*np.cos(apex/2)
+ self.ax.arrow(XY[0], XY[1], \
+ h*np.sin(course), h*np.cos(course),\
+ head_width=5, head_length=10, fc='k', ec='k')
+ self.ax.annotate('HOME', xy = (0, 0))
+ if isinstance(traj, traj_ellipse):
+ self.ax.annotate('ELLIPSE', xy = (traj.XYoff[0], traj.XYoff[1]))
+ self.ax.plot(0, 0, 'kx', ms=10, mew=2)
+ self.ax.plot(traj.XYoff[0], traj.XYoff[1], 'kx', ms=10, mew=2)
+ elif isinstance(traj, traj_sin):
+ self.ax.annotate('SIN', xy = (traj.XYoff[0], traj.XYoff[1]))
+ self.ax.plot(0, 0, 'kx', ms=10, mew=2)
+ self.ax.plot(traj.XYoff[0], traj.XYoff[1], 'kx', ms=10, mew=2)
+ elif isinstance(traj, traj_line):
+ self.ax.annotate('LINE', xy = (traj.XYoff[0], traj.XYoff[1]))
+ self.ax.plot(0, 0, 'kx', ms=10, mew=2)
+ self.ax.plot(traj.XYoff[0], traj.XYoff[1], 'kx', ms=10, mew=2)
+
+ self.ax.set_xlabel('South [m]')
+ self.ax.set_ylabel('West [m]')
+ self.ax.set_title('2D Map')
+ self.ax.set_xlim(self.XYoff[0]-0.5*np.sqrt(self.area), \
+ self.XYoff[0]+0.5*np.sqrt(self.area))
+ self.ax.set_ylim(self.XYoff[1]-0.5*np.sqrt(self.area), \
+ self.XYoff[1]+0.5*np.sqrt(self.area))
+ self.ax.axis('equal')
+ self.ax.grid()
+
+class traj_line:
+ def float_range(self, start, end, step):
+ while start <= end:
+ yield start
+ start += step
+
+ def __init__(self, Xminmax, a, b, alpha):
+ self.XYoff = np.array([a, b])
+ self.Xminmax = Xminmax
+ self.a, self.b, self.alpha = a, b, alpha
+ self.traj_points = np.zeros((2, 200))
+ self.mapgrad_X = []
+ self.mapgrad_Y = []
+ self.mapgrad_U = []
+ self.mapgrad_V = []
+
+ i = 0
+ for t in self.float_range(0, 1, 0.005):
+ x = (self.Xminmax[1]-self.Xminmax[0])*t + self.Xminmax[0]
+ i = i + 1
+
+ xtr = np.linspace(-200, 200, 400)
+
+ xl = xtr*np.sin(self.alpha) + a
+ yl = xtr*np.cos(self.alpha) + b
+
+ self.traj_points = np.vstack((xl, yl))
+
+ def param_point(self, t):
+ i = 0
+
+ def vector_field(self, XYoff, area, s, kn, ke):
+ self.mapgrad_X, self.mapgrad_Y = np.mgrid[XYoff[0]-0.5*np.sqrt(area):\
+ XYoff[0]+0.5*np.sqrt(area):30j, \
+ XYoff[1]-0.5*np.sqrt(area):\
+ XYoff[1]+0.5*np.sqrt(area):30j]
+
+ nx = -np.cos(self.alpha)
+ ny = np.sin(self.alpha)
+ tx = s*ny
+ ty = -s*nx
+
+ ke = 1e-2*ke
+
+ e = (self.mapgrad_X-self.a)*nx + (self.mapgrad_Y-self.b)*ny
+
+ self.mapgrad_U = tx -ke*e*nx
+ self.mapgrad_V = ty -ke*e*ny
+
+ norm = np.sqrt(self.mapgrad_U**2 + self.mapgrad_V**2)
+
+ self.mapgrad_U = self.mapgrad_U/norm
+ self.mapgrad_V = self.mapgrad_V/norm
+
+class traj_ellipse:
+ def float_range(self, start, end, step):
+ while start <= end:
+ yield start
+ start += step
+
+ def __init__(self, XYoff, rot, a, b):
+ self.XYoff = XYoff
+ self.a, self.b = a, b
+ self.rot = rot
+ self.traj_points = np.zeros((2, 200))
+ self.mapgrad_X = []
+ self.mapgrad_Y = []
+ self.mapgrad_U = []
+ self.mapgrad_V = []
+
+ i = 0
+ for t in self.float_range(0, 1, 0.005):
+ self.traj_points[:, i] = self.param_point(t)
+ i = i + 1
+
+ def param_point(self, t):
+ angle = 2*np.pi*t
+ return self.XYoff \
+ + np.array([self.a*np.cos(angle)*np.cos(-self.rot) - \
+ self.b*np.sin(angle)*np.sin(-self.rot), \
+ self.a*np.cos(angle)*np.sin(-self.rot) + \
+ self.b*np.sin(angle)*np.cos(-self.rot)])
+
+ def vector_field(self, XYoff, area, s, kn, ke):
+ self.mapgrad_X, self.mapgrad_Y = np.mgrid[XYoff[0]-0.5*np.sqrt(area):\
+ XYoff[0]+0.5*np.sqrt(area):30j, \
+ XYoff[1]-0.5*np.sqrt(area):\
+ XYoff[1]+0.5*np.sqrt(area):30j]
+
+ Xel = (self.mapgrad_X-self.XYoff[0])*np.cos(self.rot) \
+ - (self.mapgrad_Y-self.XYoff[1])*np.sin(self.rot)
+
+ Yel = (self.mapgrad_X-self.XYoff[0])*np.sin(self.rot) \
+ + (self.mapgrad_Y-self.XYoff[1])*np.cos(self.rot)
+
+ nx = 2*Xel*np.cos(self.rot)/self.a**2 \
+ + 2*Yel*np.sin(self.rot)/self.b**2
+ ny = -2*Xel*np.sin(self.rot)/self.a**2 \
+ + 2*Yel*np.cos(self.rot)/self.b**2
+
+ tx = s*ny
+ ty = -s*nx
+
+ e = (Xel/self.a)**2 + (Yel/self.b)**2 - 1
+
+ self.mapgrad_U = tx -ke*e*nx
+ self.mapgrad_V = ty -ke*e*ny
+
+ norm = np.sqrt(self.mapgrad_U**2 + self.mapgrad_V**2)
+
+ self.mapgrad_U = self.mapgrad_U/norm
+ self.mapgrad_V = self.mapgrad_V/norm
+
+class traj_sin:
+ def float_range(self, start, end, step):
+ while start <= end:
+ yield start
+ start += step
+
+ def __init__(self, Xminmax, a, b, alpha, w, off, A):
+ self.XYoff = np.array([a, b])
+ self.Xminmax = Xminmax
+ self.a, self.b, self.alpha, self.w, self.off, self.A = \
+ a, b, alpha, w, off, A
+ self.traj_points = np.zeros((2, 200))
+ self.mapgrad_X = []
+ self.mapgrad_Y = []
+ self.mapgrad_U = []
+ self.mapgrad_V = []
+
+ i = 0
+ for t in self.float_range(0, 1, 0.005):
+ x = (self.Xminmax[1]-self.Xminmax[0])*t + self.Xminmax[0]
+ i = i + 1
+
+ xtr = np.linspace(-200, 200, 400)
+ ytr = self.A*np.sin(self.w*xtr + self.off)
+
+ xsin = -(xtr-a)*np.sin(self.alpha) + (ytr-b)*np.cos(self.alpha)
+ ysin = (xtr-a)*np.cos(self.alpha) + (ytr-b)*np.sin(self.alpha)
+
+ self.traj_points = np.vstack((xsin, ysin))
+
+ def param_point(self, t):
+ i = 0
+
+ def vector_field(self, XYoff, area, s, kn, ke):
+ self.mapgrad_X, self.mapgrad_Y = np.mgrid[XYoff[0]-0.5*np.sqrt(area):\
+ XYoff[0]+0.5*np.sqrt(area):30j, \
+ XYoff[1]-0.5*np.sqrt(area):\
+ XYoff[1]+0.5*np.sqrt(area):30j]
+
+ xs = (self.mapgrad_X-self.XYoff[0])*np.sin(self.alpha) \
+ - (self.mapgrad_Y-self.XYoff[1])*np.cos(self.alpha)
+
+ ys = -(self.mapgrad_X-self.XYoff[0])*np.cos(self.alpha) \
+ - (self.mapgrad_Y-self.XYoff[1])*np.sin(self.alpha)
+
+ ang = self.w*xs + self.off
+
+ nx = -np.cos(self.alpha) - \
+ self.A*self.w*np.cos(ang)*np.sin(self.alpha)
+ ny = -np.sin(self.alpha) + \
+ self.A*self.w*np.cos(ang)*np.cos(self.alpha)
+ tx = s*ny
+ ty = -s*nx
+
+ ke = 1e-2*ke
+
+ e = ys - self.A*np.sin(ang)
+
+ self.mapgrad_U = tx -ke*e*nx
+ self.mapgrad_V = ty -ke*e*ny
+
+ norm = np.sqrt(self.mapgrad_U**2 + self.mapgrad_V**2)
+
+ self.mapgrad_U = self.mapgrad_U/norm
+ self.mapgrad_V = self.mapgrad_V/norm