Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-29 02:38:07 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

Make half turn in poly survey optional

Browse Source
This commit is contained in:
podhrmic
2016-05-04 19:01:46 -07:00
parent f5a37d0d88
commit ea6df39d9e
4 changed files with 25 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml
+2
View File
@@ -34,10 +34,12 @@ RP3 Lisa MX
</module> </module>
<define name="AGR_CLIMB" /> <define name="AGR_CLIMB" />
<define name="POLY_OSAM_HALF_SWEEP_ENABLED" value="FALSE"/>
</firmware> </firmware>
<modules> <modules>
<module name="nav" type="line"/> <module name="nav" type="line"/>
<module name="nav" type="survey_poly_osam"/>
<module name="sys_mon"/> <module name="sys_mon"/>
</modules> </modules>
conf/modules/nav_survey_poly_osam.xml
+2
View File
@@ -21,6 +21,8 @@ You can use:
<dl_settings NAME="P_Survey"> <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="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_setting min="0" max="500" step="1" var="Poly_Sweep" type="float" shortname="Sweep" module="modules/nav/nav_survey_poly_osam"/>
<dl_setting min="0" max="1" step="1" values="FALSE|TRUE" var="Half_Sweep_Enabled" type="bool" shortname="Half Sweep" module="modules/nav/nav_survey_poly_osam"/>
<dl_setting min="0" max="1" step="1" values="FALSE|TRUE" var="Reset_Sweep" type="bool" shortname="Reset Sweep" module="modules/nav/nav_survey_poly_osam" handler="ResetSweepNumber"/>
</dl_settings> </dl_settings>
</dl_settings> </dl_settings>
</settings> </settings>
sw/airborne/modules/nav/nav_survey_poly_osam.c
+17 -1
View File
@@ -67,9 +67,16 @@
#define POLY_OSAM_USE_FULL_CIRCLE TRUE #define POLY_OSAM_USE_FULL_CIRCLE TRUE
#endif #endif
// use half sweep at the end of polygon
#ifndef POLY_OSAM_HALF_SWEEP_ENABLED
#define POLY_OSAM_HALF_SWEEP_ENABLED TRUE
#endif
uint8_t Poly_Size = POLY_OSAM_DEFAULT_SIZE; uint8_t Poly_Size = POLY_OSAM_DEFAULT_SIZE;
float Poly_Sweep = POLY_OSAM_DEFAULT_SWEEP; float Poly_Sweep = POLY_OSAM_DEFAULT_SWEEP;
bool use_full_circle = POLY_OSAM_USE_FULL_CIRCLE; bool use_full_circle = POLY_OSAM_USE_FULL_CIRCLE;
bool Half_Sweep_Enabled = POLY_OSAM_HALF_SWEEP_ENABLED;
bool Reset_Sweep = FALSE;
bool nav_survey_poly_osam_setup_towards(uint8_t FirstWP, uint8_t Size, float Sweep, int SecondWP) bool nav_survey_poly_osam_setup_towards(uint8_t FirstWP, uint8_t Size, float Sweep, int SecondWP)
{ {
@@ -91,6 +98,14 @@ static void RotateAndTranslateToWorld(struct Point2D *p, float Zrot, float trans
static void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2); static void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2);
static float EvaluateLineForX(float y, struct Line L); static float EvaluateLineForX(float y, struct Line L);
void nav_survey_poly_osam_ResetSweepNumber(bool rst)
{
if (rst) {
PolySurveySweepBackNum = 0;
Reset_Sweep = FALSE;
}
}
#define PolygonSize POLY_OSAM_POLYGONSIZE #define PolygonSize POLY_OSAM_POLYGONSIZE
#define MaxFloat 1000000000 #define MaxFloat 1000000000
#define MinFloat -1000000000 #define MinFloat -1000000000
@@ -409,7 +424,8 @@ bool nav_survey_poly_osam_run(void)
LastPoint.y = SurveyToWP.y; LastPoint.y = SurveyToWP.y;
if (LastPoint.y + dSweep >= MaxY || LastPoint.y + dSweep <= 0) { //Your out of the Polygon so Sweep Back or Half Sweep 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
if (LastPoint.y + (dSweep / 2) >= MaxY || LastPoint.y + (dSweep / 2) <= 0 || !Half_Sweep_Enabled) { //Sweep back
dSweep = -dSweep; dSweep = -dSweep;
if (LastHalfSweep) { if (LastHalfSweep) {
HalfSweep = false; HalfSweep = false;
sw/airborne/modules/nav/nav_survey_poly_osam.h
+4
View File
@@ -33,6 +33,8 @@ extern uint8_t Poly_Size;
extern float Poly_Sweep; extern float Poly_Sweep;
extern uint16_t PolySurveySweepNum; extern uint16_t PolySurveySweepNum;
extern uint16_t PolySurveySweepBackNum; extern uint16_t PolySurveySweepBackNum;
extern bool Half_Sweep_Enabled;
extern bool Reset_Sweep;
/** /**
* Setup polygon survey. * Setup polygon survey.
@@ -59,4 +61,6 @@ extern bool nav_survey_poly_osam_setup_towards(uint8_t FirstWP, uint8_t Size, fl
/** Run polygon survey */ /** Run polygon survey */
extern bool nav_survey_poly_osam_run(void); extern bool nav_survey_poly_osam_run(void);
/** Reset sweep number */
extern void nav_survey_poly_osam_ResetSweepNumber(bool rst);
#endif #endif