[NAV] Polygon Survey with orientation defined by a waypoint

paparazzi · Oct 23, 2014 · a390d83 · a390d83
1 parent c49f516
commit a390d83
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 0 deletions.
diff --git a/sw/airborne/modules/nav/nav_survey_poly_osam.c b/sw/airborne/modules/nav/nav_survey_poly_osam.c
@@ -32,6 +32,16 @@
 #include "generated/flight_plan.h"
 
 
+bool_t nav_survey_poly_osam_setup_towards(uint8_t FirstWP, uint8_t Size, float Sweep, int SecondWP)
+{
+  float dx = waypoints[SecondWP].x - waypoints[FirstWP].x;
+  float dy = waypoints[SecondWP].y - waypoints[FirstWP].y;
+  if (dx == 0.0f) dx = 0.000000001;
+  float ang = atan(dy/dx);
+  return nav_survey_poly_osam_setup_towards(FirstWP, Size, Sweep, DegOfRad(ang));
+}
+
+
 struct Point2D {float x; float y;};
 struct Line {float m;float b;float x;};
 

diff --git a/sw/airborne/modules/nav/nav_survey_poly_osam.h b/sw/airborne/modules/nav/nav_survey_poly_osam.h
@@ -30,6 +30,8 @@
 #include "std.h"
 
 extern bool_t nav_survey_poly_osam_setup(uint8_t FirstWP, uint8_t Size, float Sweep, float Orientation);
+/** start a polygonsurvey and compute the orientation angle from the line FirstWP-SecondWP */
+extern bool_t nav_survey_poly_osam_setup_towards(uint8_t FirstWP, uint8_t Size, float Sweep, int SecondWP);
 extern bool_t nav_survey_poly_osam_run(void);
 extern uint16_t PolySurveySweepNum;
 extern uint16_t PolySurveySweepBackNum;