Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 22:17:01 +08:00
Code Issues Actions 31 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
sw/airborne/modules/nav/nav_survey_poly_osam.c
+144 -34
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
{
dSweep = -dSweep;
ys = LastPoint.y+(dSweep/2);
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
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