Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-26 16:30:07 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

Poly survey via Guidance Vector Field addition (#2052)

Browse Source 
* [GVF] more functions exposed to the user

* [GVF] gvf_line_xy1_xy2 added

* [GVF] segment msg added for the GCS

* poly survey with GVF

* [GVF] segment primitive added

* integrating gvf and poly survey

* [GVF] each trajectory has its own gains now

* updated settings (gains) for the GVF trajectories

* [GVF] nav_survey_polygon integrated with GVF

* [GVF] python scripts updated for the new GVF msg format

* pprz c code style

* [GVF] updated demo

* gvf demo includes poly survey

* [GVF] gvf poly survey moved to gvf module

* [GVF] nav module not necessary anymore for gvf demo

* [GVF] _gvf added for being different from others nav functions from other modules

* [GVF] description about employing gvf here has been removed

* gvf_ for not conflict with former nav module

* These definitions now have to be done in the airframe conf

* [GVF] kn is not used anymore by the gvf visualization

* [GVF] better description msg for the formation script

* [GVF] Doxygen doc

* print for python 3

* correct style

* we set up correctly the vertical control

* Doxygen

* Only for auto2. If we are in auto1, do not override the roll set point

* Circular formation now also works for counter-clockwise direction

* update pprzlink

* Gautier comments

* [GVF] more functions exposed to the user

* [GVF] gvf_line_xy1_xy2 added

* [GVF] segment msg added for the GCS

* poly survey with GVF

* [GVF] segment primitive added

* integrating gvf and poly survey

* [GVF] each trajectory has its own gains now

* updated settings (gains) for the GVF trajectories

* [GVF] nav_survey_polygon integrated with GVF

* [GVF] python scripts updated for the new GVF msg format

* pprz c code style

* [GVF] updated demo

* gvf demo includes poly survey

* [GVF] gvf poly survey moved to gvf module

* [GVF] nav module not necessary anymore for gvf demo

* [GVF] _gvf added for being different from others nav functions from other modules

* [GVF] description about employing gvf here has been removed

* gvf_ for not conflict with former nav module

* These definitions now have to be done in the airframe conf

* [GVF] kn is not used anymore by the gvf visualization

* [GVF] better description msg for the formation script

* [GVF] Doxygen doc

* print for python 3

* correct style

* we set up correctly the vertical control

* Doxygen

* Only for auto2. If we are in auto1, do not override the roll set point

* Circular formation now also works for counter-clockwise direction

* update pprzlink

* Gautier comments

* PPRZ_MODE_AUTO2 is now AP_MODE_AUTO2

* new functions for the GVF demo

* Primitives for tracking lines, segments and segment_loops

* Poly survey track the proper segment function
This commit is contained in:
Hector Garcia de Marina
2017-06-20 02:43:10 +02:00
committed by Michal Podhradsky
parent adc962a257
commit 2144be74be
18 changed files with 868 additions and 161 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/flight_plans/demo_gvf.xml
+40 -17
View File
@@ -1,7 +1,9 @@
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
<flight_plan alt="260" ground_alt="185" lat0="43.46223" lon0="1.27289" max_dist_from_home="1500" name="Basic" security_height="25">
<flight_plan alt="260" ground_alt="185" lat0="43.46223" lon0="1.27289" max_dist_from_home="1500" name="Fixed wing basic (Muret)" security_height="25">
<header>
</header>
<waypoints>
<waypoint name="HOME" x="0" y="0"/>
<waypoint alt="235" name="STDBY" x="49.5" y="100.1"/>
@@ -9,11 +11,27 @@
<waypoint alt="185.0" name="TD" x="28.8" y="57.0"/>
<waypoint name="_BASELEG" x="168.8" y="-13.8"/>
<waypoint name="CLIMB" x="-114.5" y="162.3"/>
<waypoint alt="235.0" name="CIRCLE" x="-83.9" y="89.4"/>
<waypoint alt="235.0" name="ELLIPSE" x="0.5" y="109.0"/>
<waypoint alt="235.0" name="S1" x="-14.8" y="157.8"/>
<waypoint alt="235.0" name="S2" x="175.7" y="85.2"/>
<waypoint alt="235.0" name="S3" x="140.0" y="-2.3"/>
<waypoint alt="235.0" name="S4" x="-49.6" y="66.3"/>
</waypoints>
<modules>
<sectors>
<sector color="orange" name="Survey">
<corner name="S1"/>
<corner name="S2"/>
<corner name="S3"/>
<corner name="S4"/>
</sector>
</sectors>
<variables>
<variable var="angle_ps" init="0" min="-180" max="179" step="1"/>
</variables>
<modules>
<module name="gvf_module"/>
</modules>
@@ -37,24 +55,29 @@
<go from="HOME" pitch="15" throttle="1.0" vmode="throttle" wp="CLIMB"/>
</block>
<block group="home" key="Ctrl+a" name="Standby" strip_button="Standby" strip_icon="home.png">
<circle radius="nav_radius" wp="STDBY"/>
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
<call fun="gvf_ellipse_wp(WP_STDBY, nav_radius, nav_radius, 0)"/>
</block>
<block name="circle">
<circle alt="GetAltRef()+50" radius="nav_radius" wp="CIRCLE"/>
<block name="ellipse">
<call fun="gvf_ellipse_wp(WP_ELLIPSE, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
</block>
<block name="ellipse1">
<call fun="gvf_ellipse(WP_CIRCLE, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
<block name="segment_loop">
<call fun="gvf_segment_loop_wp1_wp2(WP_ELLIPSE, WP_STDBY, gvf_segment_par.d1, gvf_segment_par.d2)"/>
</block>
<block name="ellipse2">
<call fun="gvf_ellipse(WP_ELLIPSE, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
<block name="segment">
<call fun="gvf_segment_wp1_wp2(WP_ELLIPSE, WP_STDBY)"/>
</block>
<block name="line">
<call fun="gvf_line_wp_heading(WP_ELLIPSE, gvf_line_par.heading)"/>
</block>
<block name="Sinusoidal">
<call fun="gvf_sin_wp_alpha(WP_ELLIPSE, gvf_sin_par.alpha, gvf_sin_par.w, gvf_sin_par.off, gvf_sin_par.A)"/>
</block>
<block name="Poly Survey">
<call_once fun="gvf_nav_survey_polygon_setup(WP_S1, 4, angle_ps, 30, 30, 40, flight_altitude)"/>
<call fun="gvf_nav_survey_polygon_run()"/>
</block>
<block name="Line">
<call fun="gvf_line_wp_heading(WP_CIRCLE, gvf_line_par.alpha)"/>
</block>
<block name="Sinusoidal">
<call fun="gvf_sin_wp_heading(WP_CIRCLE, gvf_sin_par.alpha, gvf_sin_par.w, gvf_sin_par.off, gvf_sin_par.A)"/>
</block>
<block group="land" name="Land Right AF-TD" strip_button="Land right (wp AF-TD)" strip_icon="land-right.png">
<set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
<deroute block="land"/>
conf/modules/gvf_module.xml
+41 -30
View File
@@ -5,67 +5,78 @@
<description>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 .
</description>
<section name="Ellipse" prefix="GVF_ELLIPSE_">
<define name="KE" value="1" description="Gain for the aggresivity of the gvf"/>
<define name="KN" value="1" description="Gain for the alignment of the vehicle with the gvf"/>
<define name="A" value="80" description="Horizontal axis length of the ellipse" unit="m"/>
<define name="B" value="80" description="Vertical axis length of the ellipse" unit="m"/>
<define name="ALPHA" value="0" description="Rotation of the horizontal axis" unit="deg"/>
</section>
<section name="Line" prefix="GVF_LINE_">
<define name="KE" value="1" description="Gain for the aggresivity of the gvf"/>
<define name="KN" value="1" description="Gain for the alignment of the vehicle with the gvf"/>
<define name="HEADING" value="0" description="Desired heading for the line (0 is North, 90 is East)" unit="deg"/>
<define name="D1" value="0" description="Extra distance (w.r.t. the 1st point) to be travelled before turning around for the segment_loop" unit="m"/>
<define name="D2" value="0" description="Extra distance (w.r.t. the 2nd point) to be travelled before turning around for the segment_loop" unit="m"/>
</section>
<section name="Sinusoidal" prefix="GVF_SIN_">
<define name="KE" value="1" description="Gain for the aggresivity of the gvf"/>
<define name="KN" value="1" description="Gain for the alignment of the vehicle with the gvf"/>
<define name="ALPHA" value="0" description="Desired heading for the line (0 is East, 90 is North)" unit="deg"/>
<define name="W" value="0" description="Frequency for the sinusoidal y=Asin(Wx + OFF)" unit="rad/m"/>
<define name="OFF" value="0" description="Offset for the sinusoidal y=Asin(Wx + OFF)" unit="rad"/>
<define name="A" value="0" description="Amplitude for the sinusoidal y=Asin(Wx + OFF)" unit="m"/>
</section>
</doc>
<settings name="complete">
<settings name="GVF">
<dl_settings>
<dl_settings NAME="GVF">
<dl_settings NAME="Control">
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_control.ke" shortname="gvf_ke" param="GVF_KE"/>
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_control.kn" shortname="gvf_kn" param="GVF_KN"/>
<dl_setting MAX="1" MIN="-1" STEP="2" VAR="gvf_control.s" shortname = "direction"/>
</dl_settings>
<dl_settings NAME="Ellipse">
<dl_setting MAX="150" MIN="0.0" STEP="10" VAR="gvf_ellipse_par.a" shortname="a" param="GVF_ELLIPSE_A"/>
<dl_setting MAX="150" MIN="0.0" STEP="10" VAR="gvf_ellipse_par.b" shortname="b" param="GVF_ELLIPSE_B"/>
<dl_setting MAX="90" MIN="-90" STEP="1" VAR="gvf_ellipse_par.alpha" shortname="alpha" param="GVF_ELLIPSE_ALPHA"/>
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_ellipse_par.ke" shortname="ell_ke" param="GVF_ELLIPSE_KE"/>
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_ellipse_par.kn" shortname="ell_kn" param="GVF_ELLIPSE_KN"/>
<dl_setting MAX="150" MIN="0.0" STEP="10" VAR="gvf_ellipse_par.a" shortname="ell_a" param="GVF_ELLIPSE_A"/>
<dl_setting MAX="150" MIN="0.0" STEP="10" VAR="gvf_ellipse_par.b" shortname="ell_b" param="GVF_ELLIPSE_B"/>
<dl_setting MAX="90" MIN="-90" STEP="1" VAR="gvf_ellipse_par.alpha" shortname="ell_alpha" param="GVF_ELLIPSE_ALPHA"/>
</dl_settings>
<dl_settings NAME="Line">
<dl_setting MAX="180" MIN="-180" STEP="1" VAR="gvf_line_par.alpha" shortname="alpha" param="GVF_LINE_ALPHA"/>
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_line_par.ke" shortname="line_ke" param="GVF_LINE_KE"/>
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_line_par.kn" shortname="line_kn" param="GVF_LINE_KN"/>
<dl_setting MAX="180" MIN="-180" STEP="1" VAR="gvf_line_par.heading" shortname="line_heading" param="GVF_LINE_HEADING"/>
<dl_setting MAX="100" MIN="0" STEP="1" VAR="gvf_segment_par.d1" shortname="d1_seg" param="GVF_SEGMENT_D1"/>
<dl_setting MAX="100" MIN="0" STEP="1" VAR="gvf_segment_par.d2" shortname="d2_seg" param="GVF_SEGMENT_D2"/>
</dl_settings>
<dl_settings NAME="Sine">
<dl_setting MAX="180" MIN="-180" STEP="1" VAR="gvf_sin_par.alpha" shortname="alpha" param="GVF_SIN_ALPHA"/>
<dl_setting MAX="0.01" MIN="0" STEP="0.0001" VAR="gvf_sin_par.w" shortname="w" param="GVF_SIN_W"/>
<dl_setting MAX="6.2" MIN="0" STEP="0.002" VAR="gvf_sin_par.off" shortname="off" param="GVF_SIN_OFF"/>
<dl_setting MAX="100" MIN="0" STEP="1" VAR="gvf_sin_par.A" shortname="amplitude" param="GVF_SIN_A"/>
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_sin_par.ke" shortname="sin_ke" param="GVF_SIN_KE"/>
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_sin_par.kn" shortname="sin_kn" param="GVF_SIN_KN"/>
<dl_setting MAX="180" MIN="-180" STEP="1" VAR="gvf_sin_par.alpha" shortname="sin_alpha" param="GVF_SIN_ALPHA"/>
<dl_setting MAX="0.01" MIN="0" STEP="0.0001" VAR="gvf_sin_par.w" shortname="sin_w" param="GVF_SIN_W"/>
<dl_setting MAX="6.2" MIN="0" STEP="0.002" VAR="gvf_sin_par.off" shortname="sin_off" param="GVF_SIN_OFF"/>
<dl_setting MAX="100" MIN="0" STEP="1" VAR="gvf_sin_par.A" shortname="sin_amplitude" param="GVF_SIN_A"/>
</dl_settings>
</dl_settings>
</dl_settings>
</settings>
<settings name="control">
<dl_settings>
<dl_settings NAME="GVF">
<dl_settings NAME="Control">
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_control.ke" shortname="gvf_ke" param="GVF_KE"/>
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_control.kn" shortname="gvf_kn" param="GVF_KN"/>
<dl_setting MAX="1" MIN="-1" STEP="2" VAR="gvf_control.s" shortname="direction"/>
</dl_settings>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="gvf.h"/>
<file name="trajectories/gvf_line.h"/>
<file name="trajectories/gvf_sin.h"/>
<file name="trajectories/gvf_ellipse.h"/>
<file name="nav/nav_survey_polygon_gvf.h"/>
</header>
<init fun = "gvf_init()"/>
<makefile firmware="fixedwing">
<define name="GVF_ELLIPSE_A" value="80"/>
<define name="GVF_ELLIPSE_B" value="80"/>
<define name="GVF_ELLIPSE_ALPHA" value="0"/>
<define name="GVF_SIN_ALPHA" value="0"/>
<define name="GVF_SIN_W" value="0"/>
<define name="GVF_SIN_OFF" value="0"/>
<define name="GVF_SIN_A" value="0"/>
<define name="GVF_LINE_ALPHA" value="0"/>
<file name="gvf.c"/>
<file name="trajectories/gvf_line.c"/>
<file name="trajectories/gvf_sin.c"/>
<file name="trajectories/gvf_ellipse.c"/>
<file name="nav/nav_survey_polygon_gvf.c"/>
</makefile>
</module>
conf/modules/nav_survey_polygon.xml
+2 -2
View File
@@ -14,12 +14,12 @@
- shot_dist distance between the shots
- min_rad minimal radius when navigating
- altitude the altitude that must be reached before the flyover starts
2. Run the survey with nav_survey_polygon_run()
2. Run the survey with nav_survey_polygon_run() for employing the carrot guidance algorithm
<!--
Block example:
@verbatim
<block name="Poly Survey" strip_button="Poly Survey">
<call fun="nav_survey_polygon_setup(first_wp, size, angle, sweep_width, shot_dist, min_rad, altitude)"/>
<call_once fun="nav_survey_polygon_setup(first_wp, size, angle, sweep_width, shot_dist, min_rad, altitude)"/>
<call fun="nav_survey_polygon_run()"/>
</block>
@endverbatim
sw/airborne/modules/guidance/gvf/gvf.c
+182 -35
View File
@@ -38,6 +38,7 @@ gvf_con gvf_control;
// Trajectory
gvf_tra gvf_trajectory;
gvf_seg gvf_segment;
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
@@ -65,7 +66,7 @@ static void send_gvf(struct transport_tx *trans, struct link_device *dev)
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);
&gvf_control.s, &gvf_control.ke, plen, gvf_trajectory.p);
}
static void send_circle(struct transport_tx *trans, struct link_device *dev)
@@ -78,8 +79,48 @@ static void send_circle(struct transport_tx *trans, struct link_device *dev)
}
}
static void send_segment(struct transport_tx *trans, struct link_device *dev)
{
if (gvf_trajectory.type == LINE && gvf_segment.seg == 1) {
pprz_msg_send_SEGMENT(trans, dev, AC_ID,
&gvf_segment.x1, &gvf_segment.y1,
&gvf_segment.x2, &gvf_segment.y2);
}
}
#endif
static int out_of_segment_area(float x1, float y1, float x2, float y2, float d1, float d2)
{
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 alpha = atan2f(zy, zx);
float cosa = cosf(-alpha);
float sina = sinf(-alpha);
float pxr = px * cosa - py * sina;
float zxr = zx * cosa - zy * sina;
int s = 0;
if (pxr < -d1) {
s = 1;
} else if (pxr > (zxr + d2)) {
s = -1;
}
if (zy < 0) {
s *= -1;
}
return s;
}
void gvf_init(void)
{
gvf_control.ke = 1;
@@ -89,8 +130,8 @@ void gvf_init(void)
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GVF, send_gvf);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CIRCLE,
send_circle);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CIRCLE, send_circle);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_SEGMENT, send_segment);
#endif
}
@@ -152,17 +193,13 @@ void gvf_control_2D(float ke, float kn, float e,
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);
if (autopilot_get_mode() == AP_MODE_AUTO2) {
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;
lateral_mode = LATERAL_MODE_ROLL;
}
}
void gvf_set_direction(int8_t s)
@@ -172,7 +209,7 @@ void gvf_set_direction(int8_t s)
// STRAIGHT LINE
void gvf_line(float a, float b, float alpha)
static void gvf_line(float a, float b, float heading)
{
float e;
struct gvf_grad grad_line;
@@ -181,12 +218,52 @@ void gvf_line(float a, float b, float alpha)
gvf_trajectory.type = 0;
gvf_trajectory.p[0] = a;
gvf_trajectory.p[1] = b;
gvf_trajectory.p[2] = alpha;
gvf_trajectory.p[2] = heading;
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.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;
horizontal_mode = HORIZONTAL_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;
float alpha = atanf(zx / zy);
float beta = atan2f(zy, zx);
float cosb = cosf(-beta);
float sinb = sinf(-beta);
float zxr = zx * cosb - zy * sinb;
if((zxr > 0 && zy > 0) || (zxr < 0 && zy < 0)) {
gvf_set_direction(1);
} else {
gvf_set_direction(-1);
}
gvf_line(x1, y1, alpha);
horizontal_mode = HORIZONTAL_MODE_ROUTE;
gvf_segment.seg = 1;
gvf_segment.x1 = x1;
gvf_segment.y1 = y1;
gvf_segment.x2 = x2;
gvf_segment.y2 = y2;
return true;
}
bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2)
@@ -196,38 +273,95 @@ bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2)
float x2 = waypoints[wp2].x;
float y2 = waypoints[wp2].y;
float zx = x1 - x2;
float zy = y1 - y2;
return gvf_line_XY1_XY2(x1, y1, x2, y2);
}
float alpha = atanf(zy / zx);
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);
horizontal_mode = HORIZONTAL_MODE_ROUTE;
gvf_segment.seg = 1;
gvf_segment.x1 = x1;
gvf_segment.y1 = y1;
gvf_segment.x2 = x2;
gvf_segment.y2 = y2;
return true;
}
bool gvf_line_wp_heading(uint8_t wp, float alpha)
bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2)
{
alpha = alpha * M_PI / 180;
float x1 = waypoints[wp1].x;
float y1 = waypoints[wp1].y;
float x2 = waypoints[wp2].x;
float y2 = waypoints[wp2].y;
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 = waypoints[wp1].x;
float y1 = waypoints[wp1].y;
float x2 = waypoints[wp2].x;
float y2 = waypoints[wp2].y;
return gvf_segment_XY1_XY2(x1, y1, x2, y2);
}
bool gvf_line_wp_heading(uint8_t wp, float heading)
{
heading = heading * M_PI / 180;
float a = waypoints[wp].x;
float b = waypoints[wp].y;
gvf_line(a, b, alpha);
return true;
return gvf_line_XY_heading(a, b, heading);
}
// ELLIPSE
bool gvf_ellipse(uint8_t wp, float a, float b, float alpha)
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] = waypoints[wp].x;
gvf_trajectory.p[1] = waypoints[wp].y;
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;
@@ -238,22 +372,32 @@ bool gvf_ellipse(uint8_t wp, float a, float b, float alpha)
gvf_trajectory.p[3] = 60;
}
if (gvf_trajectory.p[2] == gvf_trajectory.p[3])
if (gvf_trajectory.p[2] == gvf_trajectory.p[3]) {
horizontal_mode = HORIZONTAL_MODE_CIRCLE;
else
} else {
horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
}
gvf_ellipse_info(&e, &grad_ellipse, &Hess_ellipse);
gvf_control_2D(gvf_control.ke, gvf_control.kn, 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(waypoints[wp].x, waypoints[wp].y, a, b, alpha);
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)
bool gvf_sin_XY_alpha(float a, float b, float alpha, float w, float off, float A)
{
float e;
struct gvf_grad grad_line;
@@ -268,9 +412,12 @@ void gvf_sin(float a, float b, float alpha, float w, float off, float A)
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.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)
@@ -287,12 +434,12 @@ bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off, float A)
float alpha = atanf(zy / zx);
gvf_sin(x1, y1, alpha, w, off, A);
gvf_sin_XY_alpha(x1, y1, alpha, w, off, A);
return true;
}
bool gvf_sin_wp_heading(uint8_t wp, float alpha, float w, float off, float A)
bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, float A)
{
w = 2 * M_PI * w;
alpha = alpha * M_PI / 180;
@@ -300,7 +447,7 @@ bool gvf_sin_wp_heading(uint8_t wp, float alpha, float w, float off, float A)
float x = waypoints[wp].x;
float y = waypoints[wp].y;
gvf_sin(x, y, alpha, w, off, A);
gvf_sin_XY_alpha(x, y, alpha, w, off, A);
return true;
}
sw/airborne/modules/guidance/gvf/gvf.h
+38 -9
View File
@@ -20,7 +20,7 @@
*
*/
/** \file gvf.h
/** @file gvf.h
*
* Guidance algorithm based on vector fields
*/
@@ -32,10 +32,17 @@
#include "std.h"
/** @typedef gvf_conf
* @brief Parameters for the GVF
* @param ke Gain defining how agressive is the vector field
* @param kn Gain for making converge the vehile to the vector field
* @param error Error signal. It does not have any specific units. It depends on how the trajectory has been implemented. Check the specific wiki entry for each trajectory.
* @param s Defines the direction to be tracked. Its meaning depends on the trajectory and its implementation. Check the wiki entry of the GVF. It takes the values -1 or 1.
*/
typedef struct {
float error;
float ke;
float kn;
float error;
int8_t s;
} gvf_con;
@@ -53,6 +60,22 @@ typedef struct {
float p[16];
} gvf_tra;
/** @typedef gvf_seg
* @brief Struct employed by the LINE trajectory for the special case of trackinga segment, which is described by the coordinates x1, y1, x2, y2
* @param seg Tracking a segment or not
* @param x1 coordinate w.r.t. HOME
* @param y1 coordinate w.r.t. HOME
* @param x2 coordinate w.r.t. HOME
* @param y2 coordinate w.r.t. HOME
*/
typedef struct {
int seg;
float x1;
float y1;
float x2;
float y2;
} gvf_seg;
extern gvf_tra gvf_trajectory;
struct gvf_grad {
@@ -76,23 +99,29 @@ struct gvf_Hess {
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_XY_heading(float x, float y, float heading);
extern bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2);
extern bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2);
extern bool gvf_line_wp_heading(uint8_t wp, float alpha);
extern bool gvf_segment_loop_XY1_XY2(float x1, float y1, float x2, float y2, float d1, float d2);
extern bool gvf_segment_loop_wp1_wp2(uint8_t wp1, uint8_t wp2, float d1, float d2);
extern bool gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2);
extern bool gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2);
extern bool gvf_line_wp_heading(uint8_t wp, float heading);
// Ellipse
extern bool gvf_ellipse(uint8_t wp, float a, float b, float alpha);
extern bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha);
extern bool gvf_ellipse_XY(float x, float y, 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_XY_alpha(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);
extern bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off,
float A);
#endif // GVF_H
sw/airborne/modules/guidance/gvf/nav/nav_survey_polygon_gvf.c
+329
View File
@@ -0,0 +1,329 @@
/*
* Copyright (C) 2017 The Paparazzi Team
*
* 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 modules/guidance/gvf/nav_survey_polygon_gvf.c
*
* Advanced polygon survey for fixedwings from Uni Stuttgart
* adapted for being used with the Guidance Vector Field.
*
*/
#include "nav_survey_polygon_gvf.h"
#include "firmwares/fixedwing/nav.h"
#include "state.h"
#include "autopilot.h"
#include "generated/flight_plan.h"
#include "modules/guidance/gvf/gvf.h"
#ifdef DIGITAL_CAM
#include "modules/digital_cam/dc.h"
#endif
struct gvf_SurveyPolyAdv gvf_survey;
static void gvf_nav_points(struct FloatVect2 start, struct FloatVect2 end)
{
gvf_segment_XY1_XY2(start.x, start.y, end.x, end.y);
}
/**
* intercept two lines and give back the point of intersection
* @return FALSE if no intersection can be found or intersection does not lie between points a and b
* else TRUE
* @param p returns intersection
* @param x, y first line is defined by point x and y (goes through this points)
* @param a1, a2, b1, b2 second line by coordinates a1/a2, b1/b2
*/
static bool gvf_intercept_two_lines(struct FloatVect2 *p, struct FloatVect2 x, struct FloatVect2 y, float a1, float a2,
float b1, float b2)
{
float divider, fac;
divider = (((b2 - a2) * (y.x - x.x)) + ((x.y - y.y) * (b1 - a1)));
if (divider == 0) { return false; }
fac = ((y.x * (x.y - a2)) + (x.x * (a2 - y.y)) + (a1 * (y.y - x.y))) / divider;
if (fac > 1.0) { return false; }
if (fac < 0.0) { return false; }
p->x = a1 + fac * (b1 - a1);
p->y = a2 + fac * (b2 - a2);
return true;
}
/**
* intersects a line with the polygon and gives back the two intersection points
* @return TRUE if two intersection can be found, else FALSE
* @param x, y intersection points
* @param a, b define the line to intersection
*/
static bool gvf_get_two_intersects(struct FloatVect2 *x, struct FloatVect2 *y, struct FloatVect2 a, struct FloatVect2 b)
{
int i, count = 0;
struct FloatVect2 tmp;
for (i = 0; i < gvf_survey.poly_count - 1; i++)
if (gvf_intercept_two_lines(&tmp, a, b, waypoints[gvf_survey.poly_first + i].x, waypoints[gvf_survey.poly_first + i].y,
waypoints[gvf_survey.poly_first + i + 1].x, waypoints[gvf_survey.poly_first + i + 1].y)) {
if (count == 0) {
*x = tmp;
count++;
} else {
*y = tmp;
count++;
break;
}
}
//wrapover first,last polygon waypoint
if (count == 1
&& gvf_intercept_two_lines(&tmp, a, b, waypoints[gvf_survey.poly_first + gvf_survey.poly_count - 1].x,
waypoints[gvf_survey.poly_first + gvf_survey.poly_count - 1].y, waypoints[gvf_survey.poly_first].x,
waypoints[gvf_survey.poly_first].y)) {
*y = tmp;
count++;
}
if (count != 2) {
return false;
}
//change points
if (fabs(gvf_survey.dir_vec.x) > fabs(gvf_survey.dir_vec.y)) {
if ((y->x - x->x) / gvf_survey.dir_vec.x < 0.0) {
tmp = *x;
*x = *y;
*y = tmp;
}
} else if ((y->y - x->y) / gvf_survey.dir_vec.y < 0.0) {
tmp = *x;
*x = *y;
*y = tmp;
}
return true;
}
/**
* initializes the variables needed for the survey to start
* @param first_wp the first Waypoint of the polygon
* @param size the number of points that make up the polygon
* @param angle angle in which to do the flyovers
* @param sweep_width distance between the sweeps
* @param shot_dist distance between the shots
* @param min_rad minimal radius when navigating
* @param altitude the altitude that must be reached before the flyover starts
**/
void gvf_nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist,
float min_rad, float altitude)
{
int i;
struct FloatVect2 small, sweep;
float divider, angle_rad = angle / 180.0 * M_PI;
if (angle < 0.0) { angle += 360.0; }
if (angle >= 360.0) { angle -= 360.0; }
gvf_survey.poly_first = first_wp;
gvf_survey.poly_count = size;
gvf_survey.psa_sweep_width = sweep_width;
gvf_survey.psa_min_rad = min_rad;
gvf_survey.psa_shot_dist = shot_dist;
gvf_survey.psa_altitude = altitude;
gvf_survey.segment_angle = angle;
gvf_survey.return_angle = angle + 180;
if (gvf_survey.return_angle > 359) { gvf_survey.return_angle -= 360; }
if (angle <= 45.0 || angle >= 315.0) {
//north
gvf_survey.dir_vec.y = 1.0;
gvf_survey.dir_vec.x = 1.0 * tanf(angle_rad);
sweep.x = 1.0;
sweep.y = - gvf_survey.dir_vec.x / gvf_survey.dir_vec.y;
} else if (angle <= 135.0) {
//east
gvf_survey.dir_vec.x = 1.0;
gvf_survey.dir_vec.y = 1.0 / tanf(angle_rad);
sweep.y = - 1.0;
sweep.x = gvf_survey.dir_vec.y / gvf_survey.dir_vec.x;
} else if (angle <= 225.0) {
//south
gvf_survey.dir_vec.y = -1.0;
gvf_survey.dir_vec.x = -1.0 * tanf(angle_rad);
sweep.x = -1.0;
sweep.y = gvf_survey.dir_vec.x / gvf_survey.dir_vec.y;
} else {
//west
gvf_survey.dir_vec.x = -1.0;
gvf_survey.dir_vec.y = -1.0 / tanf(angle_rad);
sweep.y = 1.0;
sweep.x = - gvf_survey.dir_vec.y / gvf_survey.dir_vec.x;
}
//normalize
FLOAT_VECT2_NORMALIZE(sweep);
VECT2_SMUL(gvf_survey.rad_vec, sweep, gvf_survey.psa_min_rad);
VECT2_SMUL(gvf_survey.sweep_vec, sweep, gvf_survey.psa_sweep_width);
//begin at leftmost position (relative to gvf_survey.dir_vec)
VECT2_COPY(small, waypoints[gvf_survey.poly_first]);
divider = (gvf_survey.sweep_vec.y * gvf_survey.dir_vec.x) - (gvf_survey.sweep_vec.x * gvf_survey.dir_vec.y);
//calculate the leftmost point if one sees the dir vec as going "up" and the sweep vec as going right
if (divider < 0.0) {
for (i = 1; i < gvf_survey.poly_count; i++) {
if ((gvf_survey.dir_vec.x * (waypoints[gvf_survey.poly_first + i].y - small.y)) + (gvf_survey.dir_vec.y *
(small.x - waypoints[gvf_survey.poly_first + i].x)) > 0.0) {
VECT2_COPY(small, waypoints[gvf_survey.poly_first + i]);
}
}
} else {
for (i = 1; i < gvf_survey.poly_count; i++) {
if ((gvf_survey.dir_vec.x * (waypoints[gvf_survey.poly_first + i].y - small.y)) + (gvf_survey.dir_vec.y *
(small.x - waypoints[gvf_survey.poly_first + i].x)) > 0.0) {
VECT2_COPY(small, waypoints[gvf_survey.poly_first + i]);
}
}
}
//calculate the line the defines the first flyover
gvf_survey.seg_start.x = small.x + 0.5 * gvf_survey.sweep_vec.x;
gvf_survey.seg_start.y = small.y + 0.5 * gvf_survey.sweep_vec.y;
VECT2_SUM(gvf_survey.seg_end, gvf_survey.seg_start, gvf_survey.dir_vec);
if (!gvf_get_two_intersects(&gvf_survey.seg_start, &gvf_survey.seg_end, gvf_survey.seg_start, gvf_survey.seg_end)) {
gvf_survey.stage = gERR;
return;
}
//center of the entry circle
VECT2_DIFF(gvf_survey.entry_center, gvf_survey.seg_start, gvf_survey.rad_vec);
//fast climbing to desired altitude
NavVerticalAutoThrottleMode(0.0);
NavVerticalAltitudeMode(gvf_survey.psa_altitude, 0.0);
gvf_survey.stage = gENTRY;
}
/**
* main navigation routine. This is called periodically evaluates the current
* Position and stage and navigates accordingly.
* @returns True until the survey is finished
*/
void gvf_nav_direction_circle(float rad)
{
if (rad > 0) {
gvf_set_direction(-1);
} else {
gvf_set_direction(1);
}
}
bool gvf_nav_survey_polygon_run(void)
{
NavVerticalAutoThrottleMode(0.0);
NavVerticalAltitudeMode(gvf_survey.psa_altitude, 0.0);
//entry circle around entry-center until the desired altitude is reached
if (gvf_survey.stage == gENTRY) {
gvf_nav_direction_circle(gvf_survey.psa_min_rad);
gvf_ellipse_XY(gvf_survey.entry_center.x, gvf_survey.entry_center.y, gvf_survey.psa_min_rad, gvf_survey.psa_min_rad, 0);
if (NavCourseCloseTo(gvf_survey.segment_angle)
&& nav_approaching_xy(gvf_survey.seg_start.x, gvf_survey.seg_start.y, last_x, last_y, CARROT)
&& fabs(stateGetPositionUtm_f()->alt - gvf_survey.psa_altitude) <= 20) {
gvf_survey.stage = gSEG;
nav_init_stage();
#ifdef DIGITAL_CAM
dc_survey(gvf_survey.psa_shot_dist, gvf_survey.seg_start.x - gvf_survey.dir_vec.x * gvf_survey.psa_shot_dist * 0.5,
gvf_survey.seg_start.y - gvf_survey.dir_vec.y * gvf_survey.psa_shot_dist * 0.5);
#endif
}
}
//fly the segment until seg_end is reached
if (gvf_survey.stage == gSEG) {
gvf_nav_points(gvf_survey.seg_start, gvf_survey.seg_end);
//calculate all needed points for the next flyover
if (nav_approaching_xy(gvf_survey.seg_end.x, gvf_survey.seg_end.y, gvf_survey.seg_start.x, gvf_survey.seg_start.y, 0)) {
#ifdef DIGITAL_CAM
dc_stop();
#endif
VECT2_DIFF(gvf_survey.seg_center1, gvf_survey.seg_end, gvf_survey.rad_vec);
gvf_survey.ret_start.x = gvf_survey.seg_end.x - 2 * gvf_survey.rad_vec.x;
gvf_survey.ret_start.y = gvf_survey.seg_end.y - 2 * gvf_survey.rad_vec.y;
//if we get no intersection the survey is finished
static struct FloatVect2 sum_start_sweep;
static struct FloatVect2 sum_end_sweep;
VECT2_SUM(sum_start_sweep, gvf_survey.seg_start, gvf_survey.sweep_vec);
VECT2_SUM(sum_end_sweep, gvf_survey.seg_end, gvf_survey.sweep_vec);
if (!gvf_get_two_intersects(&gvf_survey.seg_start, &gvf_survey.seg_end, sum_start_sweep, sum_end_sweep)) {
return false;
}
gvf_survey.ret_end.x = gvf_survey.seg_start.x - gvf_survey.sweep_vec.x - 2 * gvf_survey.rad_vec.x;
gvf_survey.ret_end.y = gvf_survey.seg_start.y - gvf_survey.sweep_vec.y - 2 * gvf_survey.rad_vec.y;
gvf_survey.seg_center2.x = gvf_survey.seg_start.x - 0.5 * (2.0 * gvf_survey.rad_vec.x + gvf_survey.sweep_vec.x);
gvf_survey.seg_center2.y = gvf_survey.seg_start.y - 0.5 * (2.0 * gvf_survey.rad_vec.y + gvf_survey.sweep_vec.y);
gvf_survey.stage = gTURN1;
nav_init_stage();
}
}
//turn from stage to return
else if (gvf_survey.stage == gTURN1) {
gvf_nav_direction_circle(gvf_survey.psa_min_rad);
gvf_ellipse_XY(gvf_survey.seg_center1.x, gvf_survey.seg_center1.y, gvf_survey.psa_min_rad, gvf_survey.psa_min_rad, 0);
if (NavCourseCloseTo(gvf_survey.return_angle)) {
gvf_survey.stage = gRET;
nav_init_stage();
}
//return
} else if (gvf_survey.stage == gRET) {
gvf_nav_points(gvf_survey.ret_start, gvf_survey.ret_end);
if (nav_approaching_xy(gvf_survey.ret_end.x, gvf_survey.ret_end.y, gvf_survey.ret_start.x, gvf_survey.ret_start.y, 0)) {
gvf_survey.stage = gTURN2;
nav_init_stage();
}
//turn from return to stage
} else if (gvf_survey.stage == gTURN2) {
float rad_sur = (2 * gvf_survey.psa_min_rad + gvf_survey.psa_sweep_width) * 0.5;
gvf_nav_direction_circle(rad_sur);
gvf_ellipse_XY(gvf_survey.seg_center2.x, gvf_survey.seg_center2.y, rad_sur, rad_sur, 0);
if (NavCourseCloseTo(gvf_survey.segment_angle)) {
gvf_survey.stage = gSEG;
nav_init_stage();
#ifdef DIGITAL_CAM
dc_survey(gvf_survey.psa_shot_dist, gvf_survey.seg_start.x - gvf_survey.dir_vec.x * gvf_survey.psa_shot_dist * 0.5,
gvf_survey.seg_start.y - gvf_survey.dir_vec.y * gvf_survey.psa_shot_dist * 0.5);
#endif
}
}
return true;
}
sw/airborne/modules/guidance/gvf/nav/nav_survey_polygon_gvf.h
+92
View File
@@ -0,0 +1,92 @@
/*
* Copyright (C) 2017 The Paparazzi Team
*
* 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 modules/guidance/gvf/nav_survey_polygon_gvf.h
*
* Advanced polygon survey for fixedwings from Uni Stuttgart
* adapted to be employed with the Guidance Vector Field
*
*/
#ifndef NAV_SURVEY_POLYGON_GVF_H
#define NAV_SURVEY_POLYGON_GVF_H
#include "std.h"
#include "math/pprz_algebra_float.h"
/*
SurveyStage starts at ENTRY and than circles trought the other
states until to polygon is completely covered
ENTRY : getting in the right position and height for the first flyover
SEG : fly from seg_start to seg_end and take pictures,
then calculate navigation points of next flyover
TURN1 : do a 180° turn around seg_center1
RET : fly from ret_start to ret_end
TURN2 : do a 180° turn around seg_center2
*/
enum gvf_SurveyStage {gERR, gENTRY, gSEG, gTURN1, gRET, gTURN2};
struct gvf_SurveyPolyAdv {
/*
The following variables are set by nav_survey_polygon_start and not changed later on
*/
// precomputed vectors to ease calculations
struct FloatVect2 dir_vec;
struct FloatVect2 sweep_vec;
struct FloatVect2 rad_vec;
//the polygon from the flightplan
uint8_t poly_first;
uint8_t poly_count;
//desired properties of the flyover
float psa_min_rad;
float psa_sweep_width;
float psa_shot_dist;
float psa_altitude;
//direction for the flyover (0° == N)
int segment_angle;
int return_angle;
/*
The Following variables are dynamic, changed while navigating.
*/
enum gvf_SurveyStage stage;
// points for navigation
struct FloatVect2 seg_start;
struct FloatVect2 seg_end;
struct FloatVect2 seg_center1;
struct FloatVect2 seg_center2;
struct FloatVect2 entry_center;
struct FloatVect2 ret_start;
struct FloatVect2 ret_end;
};
extern void gvf_nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist,
float min_rad, float altitude);
void gvf_nav_direction_circle(float rad);
extern bool gvf_nav_survey_polygon_run(void);
#endif
sw/airborne/modules/guidance/gvf/trajectories/gvf_ellipse.c
+17 -1
View File
@@ -29,20 +29,36 @@
#include "subsystems/navigation/common_nav.h"
#include "gvf_ellipse.h"
#include "generated/airframe.h"
/*! Default gain ke for the ellipse trajectory */
#ifndef GVF_ELLIPSE_KE
#define GVF_ELLIPSE_KE 1
#endif
/*! Default gain kn for the ellipse trajectory */
#ifndef GVF_ELLIPSE_KN
#define GVF_ELLIPSE_KN 1
#endif
/*! Default first axis for the ellipse trajectory */
#ifndef GVF_ELLIPSE_A
#define GVF_ELLIPSE_A 80
#endif
/*! Default second axis for the ellipse trajectory */
#ifndef GVF_ELLIPSE_B
#define GVF_ELLIPSE_B 80
#endif
/*! Default orientation in degrees for the ellipse trajectory */
#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};
gvf_ell_par gvf_ellipse_par = {GVF_ELLIPSE_KE, GVF_ELLIPSE_KN,
GVF_ELLIPSE_A, GVF_ELLIPSE_B, GVF_ELLIPSE_ALPHA
};
void gvf_ellipse_info(float *phi, struct gvf_grad *grad,
struct gvf_Hess *hess)
sw/airborne/modules/guidance/gvf/trajectories/gvf_ellipse.h
+11 -1
View File
@@ -20,7 +20,7 @@
*
*/
/** \file gvf_ellipse.h
/** @file gvf_ellipse.h
*
* Guidance algorithm based on vector fields
* 2D Ellipse trajectory
@@ -31,7 +31,17 @@
#include "modules/guidance/gvf/gvf.h"
/** @typedef gvf_ell_par
* @brief Parameters for the GVF line trajectory
* @param ke Gain defining how agressive is the vector field
* @param kn Gain for making converge the vehile to the vector field
* @param a First axis of the ellipse in meters
* @param b Second axis of the ellipse in meters
* @param alpha Orientation of the ellipse in rads
*/
typedef struct {
float ke;
float kn;
float a;
float b;
float alpha;
sw/airborne/modules/guidance/gvf/trajectories/gvf_line.c
+26 -3
View File
@@ -29,12 +29,35 @@
#include "subsystems/navigation/common_nav.h"
#include "gvf_line.h"
#include "generated/airframe.h"
#ifndef GVF_LINE_ALPHA
#define GVF_LINE_ALPHA 0
/*! Gain ke for the line trajectory*/
#ifndef GVF_LINE_KE
#define GVF_LINE_KE 1
#endif
gvf_li_par gvf_line_par = {GVF_LINE_ALPHA};
/*! Gain kn for the line trajectory*/
#ifndef GVF_LINE_KN
#define GVF_LINE_KN 1
#endif
/*! Default heading in degrees for a trajectory called from gvf_line_**_HEADING */
#ifndef GVF_LINE_HEADING
#define GVF_LINE_HEADING 0
#endif
/*! In case of tracking a segment, how much distance in meters will go the vehicle beyond the point x1,y1 before turning back */
#ifndef GVF_SEGMENT_D1
#define GVF_SEGMENT_D1 0
#endif
/*! In case of tracking a segment, how much distance in meters will go the vehicle beyond the point x2,y2 before turning back */
#ifndef GVF_SEGMENT_D2
#define GVF_SEGMENT_D2 0
#endif
gvf_li_par gvf_line_par = {GVF_LINE_KE, GVF_LINE_KN, GVF_LINE_HEADING};
gvf_seg_par gvf_segment_par = {GVF_SEGMENT_D1, GVF_SEGMENT_D2};
void gvf_line_info(float *phi, struct gvf_grad *grad,
struct gvf_Hess *hess)
sw/airborne/modules/guidance/gvf/trajectories/gvf_line.h
+21 -2
View File
@@ -20,7 +20,7 @@
*
*/
/** \file gvf_line.h
/** @file gvf_line.h
*
* Guidance algorithm based on vector fields
* 2D straight line trajectory
@@ -31,11 +31,30 @@
#include "modules/guidance/gvf/gvf.h"
/** @typedef gvf_li_par
* @brief Parameters for the GVF line trajectory
* @param ke Gain defining how agressive is the vector field
* @param kn Gain for making converge the vehile to the vector field
* @param heading Heading in rads defining the orientation of the line
*/
typedef struct {
float alpha;
float ke;
float kn;
float heading;
} gvf_li_par;
/** @typedef gvf_seg_par
* @brief Parameters for the segment case of the GVF line trajectory
* @param d1 Distance beyond x1,y1 that the vehicle travels before turning back
* @param d2 Distance beyond x2,y2 that the vehicle travels before turning back
*/
typedef struct {
float d1;
float d2;
} gvf_seg_par;
extern gvf_li_par gvf_line_par;
extern gvf_seg_par gvf_segment_par;
extern void gvf_line_info(float *phi, struct gvf_grad *, struct gvf_Hess *);
sw/airborne/modules/guidance/gvf/trajectories/gvf_sin.c
+18 -1
View File
@@ -29,24 +29,41 @@
#include "subsystems/navigation/common_nav.h"
#include "gvf_sin.h"
#include "generated/airframe.h"
/*! Default gain ke for the sin trajectory*/
#ifndef GVF_SIN_KE
#define GVF_SIN_KE 1
#endif
/*! Default gain kn for the sin trajectory*/
#ifndef GVF_SIN_KN
#define GVF_SIN_KN 1
#endif
/*! Default orientation in rads for the sin trajectory function gvf_sin_**_alpha*/
#ifndef GVF_SIN_ALPHA
#define GVF_SIN_ALPHA 0
#endif
/*! Default frequency for the sin trajectory in rads*/
#ifndef GVF_SIN_W
#define GVF_SIN_W 0
#endif
/*! Default off-set in rads for the sin trajectory in rads*/
#ifndef GVF_SIN_OFF
#define GVF_SIN_OFF 0
#endif
/*! Default amplitude for the sin trajectory in meters*/
#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};
gvf_s_par gvf_sin_par = {GVF_SIN_KE, GVF_SIN_KN,
GVF_SIN_ALPHA, GVF_SIN_W, GVF_SIN_OFF, GVF_SIN_A
};
void gvf_sin_info(float *phi, struct gvf_grad *grad,
sw/airborne/modules/guidance/gvf/trajectories/gvf_sin.h
+12 -1
View File
@@ -20,7 +20,7 @@
*
*/
/** \file gvf_sin.h
/** @file gvf_sin.h
*
* Guidance algorithm based on vector fields
* 2D sinusoidal trajectory
@@ -31,7 +31,18 @@
#include "modules/guidance/gvf/gvf.h"
/** @typedef gvf_s_par
* @brief Parameters for the GVF line trajectory
* @param ke Gain defining how agressive is the vector field
* @param kn Gain for making converge the vehile to the vector field
* @param alpha Orientation in rads of the sin trajectory
* @param w Frequency in rads of the sin trajectory
* @param off Off-set in rads of the sin trajectory
* @param A Amplitude in meters of the sin trajectory
*/
typedef struct {
float ke;
float kn;
float alpha;
float w;
float off;
sw/airborne/modules/nav/nav_survey_polygon.c
+2 -3
View File
@@ -53,7 +53,7 @@ static void nav_points(struct FloatVect2 start, struct FloatVect2 end)
* @param a1, a2, b1, b2 second line by coordinates a1/a2, b1/b2
*/
static bool intercept_two_lines(struct FloatVect2 *p, struct FloatVect2 x, struct FloatVect2 y, float a1, float a2,
float b1, float b2)
float b1, float b2)
{
float divider, fac;
@@ -132,8 +132,7 @@ static bool get_two_intersects(struct FloatVect2 *x, struct FloatVect2 *y, struc
* @param min_rad minimal radius when navigating
* @param altitude the altitude that must be reached before the flyover starts
**/
void nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist,
float min_rad, float altitude)
void nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist, float min_rad, float altitude)
{
int i;
struct FloatVect2 small, sweep;
sw/ground_segment/python/gvf/formation/ids.txt
+1 -1
View File
@@ -1 +1 @@
14 56 34
1 2 4
sw/ground_segment/python/gvf/formation/sigmas.txt
+1 -1
View File
@@ -1 +1 @@
0 90
120 120
sw/ground_segment/python/gvf/gvfFormation.py
+23 -14
View File
@@ -1,4 +1,5 @@
#!/usr/bin/env python
from __future__ import print_function
import time
import sys
@@ -21,6 +22,8 @@ class aircraft:
self.a = -999
self.b = -999
self.s = -999
self.sigma = -999
self.a_index = -999
@@ -42,24 +45,29 @@ def message_recv(ac_id, msg):
if msg.name == 'GVF':
if int(msg.get_field(1)) == 1:
param = msg.get_field(3).split(',')
param = msg.get_field(4).split(',')
ac.XYc[0] = float(param[0])
ac.XYc[1] = float(param[1])
ac.a = float(param[2])
ac.b = float(param[3])
ac.s = float(msg.get_field(2))
if msg.name == 'BAT':
ac.time = float(msg.get_field(3))
return
def formation(B, ds, radius, k):
no_telemetry = 0
for ac in list_aircraft:
if ac.a == -999 or ac.XY[0] == -999:
print "Waiting for telemetry of aircraft ", ac.id
no_telemetry = 1
if no_telemetry:
waiting_for_msgs = 0
for ac in list_aircraft:
if ac.a == -999:
print("Waiting for GVF msg of aircraft ", ac.id)
waiting_for_msgs = 1
if ac.XY[0] == -999:
print("Waiting for NAV msg of aircraft ", ac.id)
waiting_for_msgs = 1
if waiting_for_msgs == 1:
return
sigma = np.zeros(len(list_aircraft))
@@ -84,9 +92,10 @@ def formation(B, ds, radius, k):
elif error_sigma <= -np.pi:
error_sigma = error_sigma + 2*np.pi
u = -k*B.dot(error_sigma)
print list_aircraft[0].time, " ", str(error_sigma*180.0/np.pi).replace('[','').replace(']','')
u = -list_aircraft[0].s*k*B.dot(error_sigma)
print(list_aircraft[0].time, " ", str(error_sigma*180.0/np.pi).replace('[','').replace(']',''))
i = 0
for ac in list_aircraft:
@@ -107,7 +116,7 @@ def formation(B, ds, radius, k):
def main():
if len(sys.argv) != 6:
print "Usage: gvfFormationApp topology.txt desired_sigma.txt ids.txt radius k"
print("Usage: gvfFormationApp topology.txt desired_sigma.txt ids.txt radius k")
interface.shutdown()
return
@@ -122,7 +131,7 @@ def main():
map(int, list_ids)
if np.size(ids) != np.size(B,0):
print "The ammount of aircrafts in the topology and ids does not match"
print("The ammount of aircrafts in the topology and ids does not match")
return
for i in range(0, len(ids)):
@@ -134,14 +143,14 @@ def main():
for ac in list_aircraft:
settings = PaparazziACSettings(ac.id)
list_of_indexes = ['a', 'b']
list_of_indexes = ['ell_a', 'ell_b']
for setting_ in list_of_indexes:
try:
index = settings.name_lookup[setting_].index
if setting_ == 'a':
if setting_ == 'ell_a':
ac.a_index = index
if setting_ == 'b':
if setting_ == 'ell_b':
ac.b_index = index
except Exception as e:
print(e)
sw/ground_segment/python/gvf/gvfframe.py
+12 -40
View File
@@ -37,7 +37,6 @@ class GVFFrame(wx.Frame):
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
@@ -54,23 +53,6 @@ class GVFFrame(wx.Frame):
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 int(ac_id) == self.ac_id:
@@ -81,39 +63,28 @@ class GVFFrame(wx.Frame):
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(',')]
self.ke = float(msg.get_field(3))
param = [float(x) for x in msg.get_field(4).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)
self.s, 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(',')]
self.ke = float(msg.get_field(3))
param = [float(x) for x in msg.get_field(4).split(',')]
ex = param[0]
ey = param[1]
ea = param[2]
@@ -121,13 +92,14 @@ class GVFFrame(wx.Frame):
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)
self.map_gvf.area, self.s, 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(',')]
self.ke = float(msg.get_field(3))
param = [float(x) for x in msg.get_field(4).split(',')]
a = param[0]
b = param[1]
alpha = param[2]
@@ -137,7 +109,7 @@ class GVFFrame(wx.Frame):
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.map_gvf.area, self.s, self.ke)
self.timer_traj = self.timer_traj + 1
if self.timer_traj > self.timer_traj_lim:
@@ -262,7 +234,7 @@ class traj_line:
def param_point(self, t):
i = 0
def vector_field(self, XYoff, area, s, kn, ke):
def vector_field(self, XYoff, area, s, 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):\
@@ -314,7 +286,7 @@ class traj_ellipse:
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):
def vector_field(self, XYoff, area, s, 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):\
@@ -377,7 +349,7 @@ class traj_sin:
def param_point(self, t):
i = 0
def vector_field(self, XYoff, area, s, kn, ke):
def vector_field(self, XYoff, area, s, 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):\