diff --git a/conf/modules/gvf_module.xml b/conf/modules/gvf_module.xml index 7ab2456e0f..8abe8cd89a 100644 --- a/conf/modules/gvf_module.xml +++ b/conf/modules/gvf_module.xml @@ -62,6 +62,7 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
+ @@ -72,6 +73,7 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_ + @@ -80,6 +82,7 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_ + diff --git a/sw/airborne/modules/guidance/gvf/gvf.c b/sw/airborne/modules/guidance/gvf/gvf.c index f987d5b0f7..df5d4c76b5 100644 --- a/sw/airborne/modules/guidance/gvf/gvf.c +++ b/sw/airborne/modules/guidance/gvf/gvf.c @@ -24,28 +24,18 @@ #include "std.h" #include "modules/guidance/gvf/gvf.h" +#include "modules/guidance/gvf/gvf_low_level_control.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" - -#ifdef FIXEDWING_FIRMWARE -#include "firmwares/fixedwing/nav.h" -#include "modules/nav/common_nav.h" -#include "firmwares/fixedwing/stabilization/stabilization_attitude.h" -#elif defined(ROVER_FIRMWARE) -#include "state.h" -#include "modules/nav/nav_rover_base.h" -#include "firmwares/rover/navigation.h" -struct EnuCoor_f gvf_draw_wp; -#else -#error "Firmware not supported by GVF!" -#endif - #include "autopilot.h" // Control gvf_con gvf_control; +// State +gvf_st gvf_state; + // Trajectory gvf_tra gvf_trajectory; gvf_seg gvf_segment; @@ -165,19 +155,11 @@ void gvf_control_2D(float ke, float kn, float e, struct gvf_grad *grad, struct gvf_Hess *hess) { gvf_t0 = get_sys_time_msec(); - - #if defined(FIXEDWING_FIRMWARE) - float ground_speed = stateGetHorizontalSpeedNorm_f(); - float course = stateGetHorizontalSpeedDir_f(); - float px_dot = ground_speed * sinf(course); - float py_dot = ground_speed * cosf(course); - #elif defined(ROVER_FIRMWARE) - // We assume that the course and psi - // of the rover (steering wheel) are the same - float course = stateGetNedToBodyEulers_f()->psi; - float px_dot = stateGetSpeedEnu_f()->x; - float py_dot = stateGetSpeedEnu_f()->y; - #endif + + gvf_low_level_getState(); + float course = gvf_state.course; + float px_dot = gvf_state.px_dot; + float py_dot = gvf_state.py_dot; int s = gvf_control.s; @@ -226,29 +208,9 @@ void gvf_control_2D(float ke, float kn, float e, float mr_y = cosf(course); float omega = omega_d + kn * (mr_x * md_y - mr_y * md_x); - - #if defined(FIXEDWING_FIRMWARE) - if (autopilot_get_mode() == AP_MODE_AUTO2) { - - // Coordinated turn - struct FloatEulers *att = stateGetNedToBodyEulers_f(); - float ground_speed = stateGetHorizontalSpeedNorm_f(); - - lateral_mode = LATERAL_MODE_ROLL; - - h_ctl_roll_setpoint = - -atanf(omega * ground_speed / GVF_GRAVITY / cosf(att->theta)); - BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); - } - - #elif defined(ROVER_FIRMWARE) - if (autopilot_get_mode() != AP_MODE_DIRECT) { - guidance_control.gvf_omega = omega; //TODO: set omega into external GVF variable - } - - #else - #error GVF does not support your firmware yet - #endif + + gvf_control.omega = omega; + gvf_low_level_control_2D(omega); } void gvf_set_direction(int8_t s) @@ -256,10 +218,6 @@ void gvf_set_direction(int8_t s) gvf_control.s = s; } - - -#ifdef FIXEDWING_FIRMWARE // FIXEDWING TRAJECTORIES - // STRAIGHT LINE static void gvf_line(float a, float b, float heading) @@ -279,7 +237,7 @@ static void gvf_line(float a, float b, float heading) gvf_control.error = e; - horizontal_mode = HORIZONTAL_MODE_WAYPOINT; + gvf_setNavMode(GVF_MODE_WAYPOINT); gvf_segment.seg = 0; } @@ -298,7 +256,7 @@ bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2) gvf_line_XY_heading(x1, y1, atan2f(zx, zy)); - horizontal_mode = HORIZONTAL_MODE_ROUTE; + gvf_setNavMode(GVF_MODE_ROUTE); gvf_segment.seg = 1; gvf_segment.x1 = x1; gvf_segment.y1 = y1; @@ -310,10 +268,10 @@ bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2) 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 x1 = WaypointX(wp1); + float y1 = WaypointY(wp1); + float x2 = WaypointX(wp2); + float y2 = WaypointY(wp2); return gvf_line_XY1_XY2(x1, y1, x2, y2); } @@ -331,7 +289,7 @@ bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, gvf_line(x1, y1, alpha); - horizontal_mode = HORIZONTAL_MODE_ROUTE; + gvf_setNavMode(GVF_MODE_ROUTE); gvf_segment.seg = 1; gvf_segment.x1 = x1; @@ -344,10 +302,10 @@ bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2) { - float x1 = waypoints[wp1].x; - float y1 = waypoints[wp1].y; - float x2 = waypoints[wp2].x; - float y2 = waypoints[wp2].y; + float x1 = WaypointX(wp1); + float y1 = WaypointY(wp1); + float x2 = WaypointX(wp2); + float y2 = WaypointY(wp2); return gvf_segment_loop_XY1_XY2(x1, y1, x2, y2, d1, d2); } @@ -376,10 +334,10 @@ bool gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2) bool gvf_segment_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 x1 = WaypointX(wp1); + float y1 = WaypointY(wp1); + float x2 = WaypointX(wp2); + float y2 = WaypointY(wp2); return gvf_segment_XY1_XY2(x1, y1, x2, y2); } @@ -388,8 +346,8 @@ bool gvf_line_wp_heading(uint8_t wp, float heading) { heading = RadOfDeg(heading); - float a = waypoints[wp].x; - float b = waypoints[wp].y; + float a = WaypointX(wp); + float b = WaypointY(wp); return gvf_line_XY_heading(a, b, heading); } @@ -416,10 +374,10 @@ bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha) } if ((int)gvf_trajectory.p[2] == (int)gvf_trajectory.p[3]) { - horizontal_mode = HORIZONTAL_MODE_CIRCLE; + gvf_setNavMode(GVF_MODE_CIRCLE); } else { - horizontal_mode = HORIZONTAL_MODE_WAYPOINT; + gvf_setNavMode(GVF_MODE_WAYPOINT); } gvf_ellipse_info(&e, &grad_ellipse, &Hess_ellipse); @@ -434,8 +392,8 @@ bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha) bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha) -{ - gvf_ellipse_XY(waypoints[wp].x, waypoints[wp].y, a, b, alpha); +{ + gvf_ellipse_XY(WaypointX(wp), WaypointY(wp), a, b, alpha); return true; } @@ -468,10 +426,10 @@ 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 x1 = WaypointX(wp1); + float y1 = WaypointY(wp1); + float x2 = WaypointX(wp2); + float y2 = WaypointY(wp2); float zx = x1 - x2; float zy = y1 - y2; @@ -488,267 +446,11 @@ bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A) w = 2 * M_PI * w; alpha = RadOfDeg(alpha); - float x = waypoints[wp].x; - float y = waypoints[wp].y; + float x = WaypointX(wp); + float y = WaypointY(wp); gvf_sin_XY_alpha(x, y, alpha, w, off, A); return true; } - -#elif defined(ROVER_FIRMWARE) // ROVER TRAJECTORIES - -// STRAIGHT LINE - -static void gvf_line(float a, float b, float heading) -{ - 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] = heading; - - gvf_line_info(&e, &grad_line, &Hess_line); - gvf_control.ke = gvf_line_par.ke; - gvf_control_2D(1e-2 * gvf_line_par.ke, gvf_line_par.kn, e, &grad_line, &Hess_line); - - gvf_control.error = e; - - nav.mode = NAV_MODE_WAYPOINT; - - gvf_segment.seg = 0; -} - -bool gvf_line_XY_heading(float a, float b, float heading) -{ - gvf_set_direction(1); - gvf_line(a, b, heading); - return true; -} - -bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2) -{ - float zx = x2 - x1; - float zy = y2 - y1; - - gvf_line_XY_heading(x1, y1, atan2f(zx, zy)); - - nav.mode = NAV_MODE_ROUTE; - - gvf_segment.seg = 1; - gvf_segment.x1 = x1; - gvf_segment.y1 = y1; - gvf_segment.x2 = x2; - gvf_segment.y2 = y2; - - // Send rover_base navigation data to draw segment (ROUTE) - nav_rover_base.goto_wp.from.x = x1; - nav_rover_base.goto_wp.from.y = y1; - nav_rover_base.goto_wp.to.x = x2; - nav_rover_base.goto_wp.to.y = y2; - - return true; -} - -bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2) -{ - float x1 = waypoint_get_x(wp1); - float y1 = waypoint_get_y(wp1); - float x2 = waypoint_get_x(wp2); - float y2 = waypoint_get_y(wp2); - - return gvf_line_XY1_XY2(x1, y1, x2, y2); -} - -bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2) -{ - int s = out_of_segment_area(x1, y1, x2, y2, d1, d2); - if (s != 0) { - gvf_control.s = s; - } - - float zx = x2 - x1; - float zy = y2 - y1; - float alpha = atanf(zx / zy); - - gvf_line(x1, y1, alpha); - - nav.mode = NAV_MODE_ROUTE; - - gvf_segment.seg = 1; - gvf_segment.x1 = x1; - gvf_segment.y1 = y1; - gvf_segment.x2 = x2; - gvf_segment.y2 = y2; - - // Send rover_base navigation data to draw segment (ROUTE) - nav_rover_base.goto_wp.from.x = x1; - nav_rover_base.goto_wp.from.y = y1; - nav_rover_base.goto_wp.to.x = x2; - nav_rover_base.goto_wp.to.y = y2; - - return true; -} - -bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2) -{ - float x1 = waypoint_get_x(wp1); - float y1 = waypoint_get_y(wp1); - float x2 = waypoint_get_x(wp2); - float y2 = waypoint_get_y(wp2); - - return gvf_segment_loop_XY1_XY2(x1, y1, x2, y2, d1, d2); -} - -bool gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2) -{ - struct EnuCoor_f *p = stateGetPositionEnu_f(); - float px = p->x - x1; - float py = p->y - y1; - - float zx = x2 - x1; - float zy = y2 - y1; - - float beta = atan2f(zy, zx); - float cosb = cosf(-beta); - float sinb = sinf(-beta); - float zxr = zx * cosb - zy * sinb; - float pxr = px * cosb - py * sinb; - - if ((zxr > 0 && pxr > zxr) || (zxr < 0 && pxr < zxr)) { - return false; - } - - return gvf_line_XY1_XY2(x1, y1, x2, y2); -} - -bool gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2) -{ - float x1 = waypoint_get_x(wp1); - float y1 = waypoint_get_y(wp1); - float x2 = waypoint_get_x(wp2); - float y2 = waypoint_get_y(wp2); - - return gvf_segment_XY1_XY2(x1, y1, x2, y2); -} - -bool gvf_line_wp_heading(uint8_t wp, float heading) -{ - heading = RadOfDeg(heading); - - float a = waypoint_get_x(wp); - float b = waypoint_get_y(wp); - - return gvf_line_XY_heading(a, b, heading); -} - -// ELLIPSE - -bool gvf_ellipse_XY(float x, float y, 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] = x; - gvf_trajectory.p[1] = 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; - } - - // Send rover_base navigation data to draw circle (CIRCLE) - if (gvf_trajectory.p[2] == gvf_trajectory.p[3]) { - nav.mode = NAV_MODE_CIRCLE; - nav_rover_base.circle.center.x = x; - nav_rover_base.circle.center.y = y; - nav_rover_base.circle.radius = a; - } else { - nav.mode = NAV_MODE_WAYPOINT; - } - - gvf_ellipse_info(&e, &grad_ellipse, &Hess_ellipse); - gvf_control.ke = gvf_ellipse_par.ke; - gvf_control_2D(gvf_ellipse_par.ke, gvf_ellipse_par.kn, - e, &grad_ellipse, &Hess_ellipse); - - gvf_control.error = e; - - return true; -} - - -bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha) -{ - gvf_ellipse_XY(waypoint_get_x(wp), waypoint_get_y(wp), a, b, alpha); - return true; -} - -// SINUSOIDAL (if w = 0 and off = 0, then we just have the straight line case) - -bool gvf_sin_XY_alpha(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.ke = gvf_sin_par.ke; - gvf_control_2D(1e-2 * gvf_sin_par.ke, gvf_sin_par.kn, e, &grad_line, &Hess_line); - - gvf_control.error = e; - - return true; -} - -bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A) -{ - w = 2 * M_PI * w; - - float x1 = waypoint_get_x(wp1); - float y1 = waypoint_get_y(wp1); - float x2 = waypoint_get_x(wp2); - float y2 = waypoint_get_y(wp2); - - float zx = x1 - x2; - float zy = y1 - y2; - - float alpha = atanf(zy / zx); - - gvf_sin_XY_alpha(x1, y1, alpha, w, off, A); - - return true; -} - -bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A) -{ - w = 2 * M_PI * w; - alpha = RadOfDeg(alpha); - - float x = waypoint_get_x(wp); - float y = waypoint_get_y(wp); - - gvf_sin_XY_alpha(x, y, alpha, w, off, A); - - return true; -} -#endif - diff --git a/sw/airborne/modules/guidance/gvf/gvf.h b/sw/airborne/modules/guidance/gvf/gvf.h index be824a9a27..4696236893 100644 --- a/sw/airborne/modules/guidance/gvf/gvf.h +++ b/sw/airborne/modules/guidance/gvf/gvf.h @@ -43,11 +43,20 @@ typedef struct { float ke; float kn; float error; + float omega; int8_t s; } gvf_con; extern gvf_con gvf_control; +typedef struct { + float course; + float px_dot; + float py_dot; +} gvf_st; + +extern gvf_st gvf_state; + enum trajectories { LINE = 0, ELLIPSE, diff --git a/sw/airborne/modules/guidance/gvf/gvf_low_level_control.c b/sw/airborne/modules/guidance/gvf/gvf_low_level_control.c new file mode 100644 index 0000000000..589c59d352 --- /dev/null +++ b/sw/airborne/modules/guidance/gvf/gvf_low_level_control.c @@ -0,0 +1,71 @@ +/* + * Copyright (C) 2020 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, see + * . + */ + +/** + * @file modules/guidance/gvf/gvf_low_level_control.c + * + * Firmware dependent file for the guiding vector field algorithm for 2D trajectories. + */ + +#include "autopilot.h" +#include "modules/guidance/gvf/gvf_low_level_control.h" +#include "modules/guidance/gvf/gvf.h" + +#if defined(FIXEDWING_FIRMWARE) +#include "firmwares/fixedwing/stabilization/stabilization_attitude.h" +#endif + +void gvf_low_level_getState(void) +{ + #if defined(FIXEDWING_FIRMWARE) + float ground_speed = stateGetHorizontalSpeedNorm_f(); + gvf_state.course = stateGetHorizontalSpeedDir_f(); + gvf_state.px_dot = ground_speed * sinf(course); + gvf_state.py_dot = ground_speed * cosf(course); + + #elif defined(ROVER_FIRMWARE) + // We assume that the course and psi + // of the rover (steering wheel) are the same + gvf_state.course = stateGetNedToBodyEulers_f()->psi; + gvf_state.px_dot = stateGetSpeedEnu_f()->x; + gvf_state.py_dot = stateGetSpeedEnu_f()->y; + #endif +} + +void gvf_low_level_control_2D(float omega) +{ + +#if defined(FIXEDWING_FIRMWARE) + if (autopilot_get_mode() == AP_MODE_AUTO2) { + + // Coordinated turn + struct FloatEulers *att = stateGetNedToBodyEulers_f(); + float ground_speed = stateGetHorizontalSpeedNorm_f(); + + lateral_mode = LATERAL_MODE_ROLL; + + h_ctl_roll_setpoint = + -atanf(omega * ground_speed / GVF_GRAVITY / cosf(att->theta)); + BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); + } +#endif + +} + diff --git a/sw/airborne/modules/guidance/gvf/gvf_low_level_control.h b/sw/airborne/modules/guidance/gvf/gvf_low_level_control.h new file mode 100644 index 0000000000..26c1e91a43 --- /dev/null +++ b/sw/airborne/modules/guidance/gvf/gvf_low_level_control.h @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2020 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, see + * . + */ + +/** + * @file modules/guidance/gvf/gvf_low_level_control.h + * + * Firmware dependent file for the guiding vector field algorithm for 2D trajectories. + */ + +#ifndef GVF_LOW_LEVEL_CONTROL_H +#define GVF_LOW_LEVEL_CONTROL_H + +#ifdef FIXEDWING_FIRMWARE +#include "firmwares/fixedwing/nav.h" +#include "modules/nav/common_nav.h" +#include "firmwares/fixedwing/stabilization/stabilization_attitude.h" +#define gvf_setNavMode(_navMode) (horizontal_mode = _navMode) +#define GVF_MODE_ROUTE HORIZONTAL_MODE_ROUTE +#define GVF_MODE_WAYPOINT HORIZONTAL_MODE_WAYPOINT +#define GVF_MODE_CIRCLE HORIZONTAL_MODE_CIRCLE + +#elif defined(ROVER_FIRMWARE) +#include "state.h" +#include "firmwares/rover/navigation.h" +#define gvf_setNavMode(_navMode) (nav.mode = _navMode) +#define GVF_MODE_ROUTE NAV_MODE_ROUTE +#define GVF_MODE_WAYPOINT NAV_MODE_WAYPOINT +#define GVF_MODE_CIRCLE NAV_MODE_CIRCLE + +#else +#error "GVF does not support your firmware yet!" +#endif + +// Low level control functions +extern void gvf_low_level_getState(void); +extern void gvf_low_level_control_2D(float); + +#endif // GVF_LOW_LEVEL_CONTROL_H +