diff --git a/conf/flight_plans/nav_modules.xml b/conf/flight_plans/nav_modules.xml index 0ad8e95a46..c8dc6e8cff 100644 --- a/conf/flight_plans/nav_modules.xml +++ b/conf/flight_plans/nav_modules.xml @@ -3,17 +3,17 @@
#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; -
+ + @@ -24,9 +24,8 @@ - - + @@ -34,7 +33,6 @@ - @@ -76,7 +74,6 @@ - @@ -90,7 +87,7 @@ - + @@ -104,7 +101,11 @@ - + + + + + @@ -122,8 +123,6 @@ - - diff --git a/conf/modules/nav_survey_poly_osam.xml b/conf/modules/nav_survey_poly_osam.xml index f13bf7e5cf..223582b442 100644 --- a/conf/modules/nav_survey_poly_osam.xml +++ b/conf/modules/nav_survey_poly_osam.xml @@ -3,9 +3,28 @@ - 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 + + + + + + + + + + + + + + + +
diff --git a/sw/airborne/modules/nav/nav_survey_poly_osam.c b/sw/airborne/modules/nav/nav_survey_poly_osam.c index 937c0a6607..b37d82b1f4 100644 --- a/sw/airborne/modules/nav/nav_survey_poly_osam.c +++ b/sw/airborne/modules/nav/nav_survey_poly_osam.c @@ -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: diff --git a/sw/airborne/modules/nav/nav_survey_poly_osam.h b/sw/airborne/modules/nav/nav_survey_poly_osam.h index 233d0e7b56..68880d741e 100644 --- a/sw/airborne/modules/nav/nav_survey_poly_osam.h +++ b/sw/airborne/modules/nav/nav_survey_poly_osam.h @@ -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