Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 05:42:49 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[modules] nav_survey_poly_osam: sweep improvements

Browse Source 
Added half U-turn even if not sweepback
Some modification to change sweep distance and number of points using telemetry parametrers
option to use default 1/2 sweep distance for start line or using alternative 1/32 sweep start

Its is very useful when using poligon survey osam for photogrametry

closes #938
This commit is contained in:
agressiva
2014-11-05 23:05:07 -02:00
committed by Felix Ruess
parent d148a18b60
commit 918fc957dc
4 changed files with 185 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/flight_plans/nav_modules.xml
+8 -9
View File
@@ -3,7 +3,6 @@
<flight_plan alt="260" ground_alt="185" lat0="43.46223" lon0="1.27289" max_dist_from_home="1500" name="Basic" security_height="25">
<header>
#include "subsystems/datalink/datalink.h"
#include "modules/digital_cam/dc.h"
#define LINE_START_FUNCTION dc_Survey(40);
#define LINE_STOP_FUNCTION dc_autoshoot = DC_AUTOSHOOT_STOP;
@@ -14,6 +13,7 @@
<waypoint name="1" x="10.1" y="189.9"/>
<waypoint name="2" x="132.3" y="139.1"/>
<waypoint name="MOB" x="137.0" y="-11.6"/>
<waypoint name="S1_angle" x="156.2" y="222.2"/>
<waypoint name="S1" x="-109.0" y="245.9"/>
<waypoint name="S2" x="274.4" y="209.5"/>
<waypoint name="S3" x="322.1" y="29.9"/>
@@ -24,9 +24,8 @@
<waypoint name="_BASELEG" x="168.8" y="-13.8"/>
<waypoint name="CLIMB" x="-114.5" y="162.3"/>
</waypoints>
<sectors>
<sector name="POLY_SURVEY" color="green">
<sector color="green" name="POLY_SURVEY">
<corner name="S1"/>
<corner name="S2"/>
<corner name="S3"/>
@@ -34,7 +33,6 @@
<corner name="S5"/>
</sector>
</sectors>
<exceptions/>
<blocks>
<block name="Wait GPS">
@@ -76,7 +74,6 @@
<block group="extra_pattern" name="Survey S1-S3" strip_button="Survey (wp S1-S3)" strip_icon="survey.png">
<survey_rectangle grid="150" wp1="S1" wp2="S3"/>
</block>
<!--===================== Nav modules example blocks =================================-->
<block name="Bungee take-off">
<call fun="nav_bungee_takeoff_setup(WP_HOME)"/>
<call fun="nav_bungee_takeoff_run()"/>
@@ -90,7 +87,7 @@
<call fun="nav_line_border_run(WP_1, WP_2, nav_radius)"/>
</block>
<block group="nav_pattern" name="Smooth nav (wp 1-2)" strip_button="Smooth nav">
<set var="snav_desired_tow" value="gps.tow / 1000. + 60."/>
<set value="gps.tow / 1000. + 60." var="snav_desired_tow"/>
<call fun="snav_init(WP_1, M_PI_2-atan2(WaypointY(WP_2)-WaypointY(WP_1),WaypointX(WP_2)-WaypointX(WP_1)), DEFAULT_CIRCLE_RADIUS/2.)"/>
<call fun="snav_circle1()"/>
<call fun="snav_route()"/>
@@ -104,7 +101,11 @@
<call fun="nav_flower_run()"/>
</block>
<block name="Poly survey osam">
<call fun="nav_survey_poly_osam_setup(WP_S1, 5, 100, 45)"/>
<call fun="nav_survey_poly_osam_setup(WP_S1, 5, 100, -5)"/>
<call fun="nav_survey_poly_osam_run()"/>
</block>
<block name="Poly survey osam dynamic">
<call fun="nav_survey_poly_osam_setup_towards(WP_S1, 0, 0, WP_S1_angle)"/>
<call fun="nav_survey_poly_osam_run()"/>
</block>
<block name="ZamboniSurvey">
@@ -122,8 +123,6 @@
<call fun="nav_spiral_setup(WP_S1, WP_S2, 30, 10, 3)"/>
<call fun="nav_spiral_run()"/>
</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/nav_survey_poly_osam.xml
+20 -1
View File
@@ -3,9 +3,28 @@
<module name="nav_survey_poly" dir="nav">
<doc>
<description>
Polygon survey from OSAM
Polygon survey from OSAM.
You can use:
* nav_survey_poly_osam_setup function for static definitions or
* nav_survey_poly_osam_setup_towards for a more flexible setup
</description>
<define name="POLY_OSAM_POLYGONSIZE" value="10" description="max waypoints usable in polygon survey"/>
<define name="POLY_OSAM_ENTRY_RADIUS" value="0" description="entry circle radius in meters. If 0 radius equal to half sweep"/>
<define name="POLY_OSAM_DEFAULT_SIZE" value="5" description="default number of waypoints/corners used in polygon survey. Can be changed by telemetry"/>
<define name="POLY_OSAM_DEFAULT_SWEEP" value="120" description="default distance in m between lines. Can be changed by telemetry"/>
<define name="POLY_OSAM_MIN_RADIUS" value="30" description="min radius used in U-turns"/>
<define name="POLY_OSAM_FIRST_SWEEP_DISTANCE" value="4" description="distante between the polygon border and first line.If 0 the distance well be equal to half sweep"/>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="P_Survey">
<dl_setting min="0" max="20" step="1" var="Poly_Size" type="uint8_t" shortname="Size" module="modules/nav/nav_survey_poly_osam"/>
<dl_setting min="0" max="500" step="1" var="Poly_Sweep" type="float" shortname="Sweep" module="modules/nav/nav_survey_poly_osam"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="nav_survey_poly_osam.h"/>
</header>
sw/airborne/modules/nav/nav_survey_poly_osam.c
+139 -29
View File
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2008-2013 The Paparazzi Team
* Copyright (C) 2008-2014 The Paparazzi Team
*
* This file is part of paparazzi.
*
@@ -31,6 +31,41 @@
#include "autopilot.h"
#include "generated/flight_plan.h"
#ifndef POLY_OSAM_DEFAULT_SIZE
#define POLY_OSAM_DEFAULT_SIZE 5
#endif
#ifndef POLY_OSAM_DEFAULT_SWEEP
#define POLY_OSAM_DEFAULT_SWEEP 100
#endif
/// Default entry radius, if 0 default to half sweep
#ifndef POLY_OSAM_ENTRY_RADIUS
#define POLY_OSAM_ENTRY_RADIUS 0
#endif
/// if 0 never check for min radius
#ifndef POLY_OSAM_MIN_RADIUS
#define POLY_OSAM_MIN_RADIUS 30
#endif
/// if 0 default to half sweep
#ifndef POLY_OSAM_FIRST_SWEEP_DISTANCE
#define POLY_OSAM_FIRST_SWEEP_DISTANCE 0
#endif
/// maximum number of polygon corners
#ifndef POLY_OSAM_POLYGONSIZE
#define POLY_OSAM_POLYGONSIZE 10
#endif
#ifndef POLY_OSAM_USE_FULL_CIRCLE
#define POLY_OSAM_USE_FULL_CIRCLE TRUE
#endif
uint8_t Poly_Size = POLY_OSAM_DEFAULT_SIZE;
float Poly_Sweep = POLY_OSAM_DEFAULT_SWEEP;
bool_t use_full_circle = POLY_OSAM_USE_FULL_CIRCLE;
bool_t nav_survey_poly_osam_setup_towards(uint8_t FirstWP, uint8_t Size, float Sweep, int SecondWP)
{
@@ -38,10 +73,12 @@ bool_t nav_survey_poly_osam_setup_towards(uint8_t FirstWP, uint8_t Size, float S
float dy = waypoints[SecondWP].y - waypoints[FirstWP].y;
if (dx == 0.0f) dx = 0.000000001;
float ang = atan(dy/dx);
//if values passed, use it.
if ( Size == 0 ) {Size = Poly_Size;}
if ( Sweep == 0) {Sweep = Poly_Sweep;}
return nav_survey_poly_osam_setup(FirstWP, Size, Sweep, DegOfRad(ang));
}
struct Point2D {float x; float y;};
struct Line {float m;float b;float x;};
@@ -50,7 +87,7 @@ static void RotateAndTranslateToWorld(struct Point2D *p, float Zrot, float trans
static void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2);
static float EvaluateLineForX(float y, struct Line L);
#define PolygonSize 10
#define PolygonSize POLY_OSAM_POLYGONSIZE
#define MaxFloat 1000000000
#define MinFloat -1000000000
@@ -84,6 +121,7 @@ static float SurveyCircleQdr;
static float MaxY;
uint16_t PolySurveySweepNum;
uint16_t PolySurveySweepBackNum;
float EntryRadius;
bool_t nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float Orientation)
{
@@ -97,6 +135,20 @@ bool_t nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float
float temp;
float XIntercept1 = 0;
float XIntercept2 = 0;
float entry_distance;
float PolySurveyEntryDistance = POLY_OSAM_FIRST_SWEEP_DISTANCE;
float PolySurveyEntryRadius = POLY_OSAM_ENTRY_RADIUS;
if (PolySurveyEntryDistance == 0)
entry_distance = sw/2;
else
entry_distance = PolySurveyEntryDistance;
if (PolySurveyEntryRadius == 0)
EntryRadius = sw/2;
else
EntryRadius = PolySurveyEntryRadius;
SurveyTheta = RadOfDeg(Orientation);
PolySurveySweepNum = 0;
@@ -220,9 +272,16 @@ bool_t nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float
//Find amount to increment by every sweep
if(EntryPoint.y >= MaxY/2)
{
entry_distance = -entry_distance;
EntryRadius = -EntryRadius;
dSweep = -sw;
}
else
{
EntryRadius = EntryRadius;
dSweep = sw;
}
//CircleQdr tells the plane when to exit the circle
if(dSweep >= 0)
@@ -231,7 +290,7 @@ bool_t nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float
SurveyCircleQdr = 180-DegOfRad(SurveyTheta);
//Find y value of the first sweep
ys = EntryPoint.y+(dSweep/2);
ys = EntryPoint.y+entry_distance;
//Find the edges which intercet the sweep line first
for(i = 0; i < SurveySize; i++)
@@ -263,15 +322,15 @@ bool_t nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float
//Find the direction to circle
if(ys > 0 && SurveyToWP.x > SurveyFromWP.x)
SurveyRadius = dSweep/2;
SurveyRadius = EntryRadius;
else if(ys < 0 && SurveyToWP.x < SurveyFromWP.x)
SurveyRadius = dSweep/2;
SurveyRadius = EntryRadius;
else
SurveyRadius = -dSweep/2;
SurveyRadius = -EntryRadius;
//Find the entry circle
SurveyCircle.x = SurveyFromWP.x;
SurveyCircle.y = EntryPoint.y;
SurveyCircle.y = EntryPoint.y + entry_distance - EntryRadius;
//Go into entry circle state
CSurveyStatus = Entry;
@@ -289,11 +348,14 @@ bool_t nav_survey_poly_osam_run(void)
float ys;
static struct Point2D LastPoint;
int i;
bool_t SweepingBack = FALSE;
bool_t LastHalfSweep;
static bool_t HalfSweep = FALSE;
float XIntercept1 = 0;
float XIntercept2 = 0;
float DInt1 = 0;
float DInt2 = 0;
float temp;
float min_radius = POLY_OSAM_MIN_RADIUS;
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(waypoints[SurveyEntryWP].a, 0.);
@@ -314,22 +376,38 @@ bool_t nav_survey_poly_osam_run(void)
{
CSurveyStatus = Sweep;
nav_init_stage();
LINE_START_FUNCTION;
//LINE_START_FUNCTION;
}
break;
case Sweep:
//Rotate and Translate Line points into real world
LastHalfSweep = HalfSweep;
ToP.x = SurveyToWP.x;
ToP.y = SurveyToWP.y;
FromP.x = SurveyFromWP.x;
FromP.y = SurveyFromWP.y;
//Rotate and Translate de plane position to local world
C.x = stateGetPositionEnu_f()->x;
C.y = stateGetPositionEnu_f()->y;
TranslateAndRotateFromWorld(&C, SurveyTheta, 0, 0);
TranslateAndRotateFromWorld(&C, 0, SmallestCorner.x, SmallestCorner.y);
//calc distance from line start and plane position (use only X position because y can be far due to wind or other factor)
float dist = FromP.x - C.x;
//Rotate and Translate Line points into real world
RotateAndTranslateToWorld(&ToP, 0, SmallestCorner.x, SmallestCorner.y);
RotateAndTranslateToWorld(&ToP, SurveyTheta, 0, 0);
RotateAndTranslateToWorld(&FromP, 0, SmallestCorner.x, SmallestCorner.y);
RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0);
// verify if plane are less than 10 meter from line start
if ((dc_autoshoot == DC_AUTOSHOOT_STOP) && (fabs(dist) < 10))
{
LINE_START_FUNCTION;
}
//follow the line
nav_route_xy(FromP.x,FromP.y,ToP.x,ToP.y);
if(nav_approaching_xy(ToP.x,ToP.y,FromP.x,FromP.y, 0))
@@ -337,21 +415,45 @@ bool_t nav_survey_poly_osam_run(void)
LastPoint.x = SurveyToWP.x;
LastPoint.y = SurveyToWP.y;
if(LastPoint.y+dSweep >= MaxY || LastPoint.y+dSweep <= 0) //Your out of the Polygon so Sweep Back
if(LastPoint.y+dSweep >= MaxY || LastPoint.y+dSweep <= 0) //Your out of the Polygon so Sweep Back or Half Sweep
{
if(LastPoint.y+(dSweep/2) >= MaxY || LastPoint.y+(dSweep/2) <= 0) //Sweep back
{
dSweep = -dSweep;
if (LastHalfSweep)
{
HalfSweep = FALSE;
ys = LastPoint.y+(dSweep);
}
else
{
HalfSweep = TRUE;
ys = LastPoint.y+(dSweep/2);
}
if(dSweep >= 0)
SurveyCircleQdr = -DegOfRad(SurveyTheta);
else
SurveyCircleQdr = 180-DegOfRad(SurveyTheta);
PolySurveySweepBackNum++;
}
else // Half Sweep forward
{
ys = LastPoint.y+(dSweep/2);
if(dSweep >= 0)
SurveyCircleQdr = -DegOfRad(SurveyTheta);
else
SurveyCircleQdr = 180-DegOfRad(SurveyTheta);
SweepingBack = TRUE;
PolySurveySweepBackNum++;
HalfSweep = TRUE;
}
else
}
else // Normal sweep
{
//Find y value of the first sweep
HalfSweep = FALSE;
ys = LastPoint.y+dSweep;
}
@@ -416,26 +518,34 @@ bool_t nav_survey_poly_osam_run(void)
}
}
//Find the radius to circle
if (!HalfSweep || use_full_circle)
temp = dSweep/2;
else
temp = dSweep/4;
//if less than min radius
if (fabs(temp) < min_radius)
{
if (temp < 0) temp = -min_radius; else temp = min_radius;
}
//Find the direction to circle
if(ys > 0 && SurveyToWP.x > SurveyFromWP.x)
SurveyRadius = temp;
else if(ys < 0 && SurveyToWP.x < SurveyFromWP.x)
SurveyRadius = temp;
else
SurveyRadius = -temp;
//find x position to circle
if(fabs(LastPoint.x-SurveyToWP.x) > fabs(SurveyFromWP.x-SurveyToWP.x))
SurveyCircle.x = LastPoint.x;
else
SurveyCircle.x = SurveyFromWP.x;
if(!SweepingBack)
SurveyCircle.y = LastPoint.y+(dSweep/2);
else
SurveyCircle.y = LastPoint.y;
//Find the direction to circle
if(ys > 0 && SurveyToWP.x > SurveyFromWP.x)
SurveyRadius = dSweep/2;
else if(ys < 0 && SurveyToWP.x < SurveyFromWP.x)
SurveyRadius = dSweep/2;
else
SurveyRadius = -dSweep/2;
//y position to circle
SurveyCircle.y = ys - temp;
//Go into circle state
CSurveyStatus = SweepCircle;
@@ -459,7 +569,7 @@ bool_t nav_survey_poly_osam_run(void)
{
CSurveyStatus = Sweep;
nav_init_stage();
LINE_START_FUNCTION;
//LINE_START_FUNCTION;
}
break;
case Init:
sw/airborne/modules/nav/nav_survey_poly_osam.h
+12 -7
View File
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2008-2013 The Paparazzi Team
* Copyright (C) 2008-2014 The Paparazzi Team
*
* This file is part of paparazzi.
*
@@ -29,6 +29,11 @@
#include "std.h"
extern uint8_t Poly_Size;
extern float Poly_Sweep;
extern uint16_t PolySurveySweepNum;
extern uint16_t PolySurveySweepBackNum;
/**
* Setup polygon survey.
* @param FirstWP first waypoint/corner of the polygon
@@ -39,11 +44,14 @@
extern bool_t nav_survey_poly_osam_setup(uint8_t FirstWP, uint8_t Size, float Sweep, float Orientation);
/**
* Setup polygon survey with sweep orientation towards a waypoint.
* Setup "dynamic" polygon survey with sweep orientation towards a waypoint.
* Computes the sweep orientation angle from the line FirstWP-SecondWP.
* If you pass zero for Size and/or Sweep it will use the global Poly_Size and
* Poly_Sweep variables respectively (which can be changed via telemetry/settings).
* @param FirstWp first waypoint/corner of the polygon
* @param Size number of waypoints/corners used to define the polygon
* @param Sweep distance between scan lines
* @param Size number of waypoints/corners used to define the polygon,
* if zero uses Poly_Size
* @param Sweep distance between scan lines, if zero uses Poly_Sweep
* @param SecondWp second waypoint towards which the sweep orientation is computed
*/
extern bool_t nav_survey_poly_osam_setup_towards(uint8_t FirstWP, uint8_t Size, float Sweep, int SecondWP);
@@ -51,7 +59,4 @@ extern bool_t nav_survey_poly_osam_setup_towards(uint8_t FirstWP, uint8_t Size,
/** Run polygon survey */
extern bool_t nav_survey_poly_osam_run(void);
extern uint16_t PolySurveySweepNum;
extern uint16_t PolySurveySweepBackNum;
#endif