|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
This routine will cover the enitre area of any Polygon defined in the flightplan which is a convex polygon. More...
#include "modules/nav/nav_survey_hybrid.h"#include "firmwares/rotorcraft/navigation.h"#include "modules/nav/nav_rotorcraft_base.h"#include "math/pprz_algebra_float.h"#include "state.h"#include "autopilot.h"#include "generated/flight_plan.h"Go to the source code of this file.
Data Structures | |
| struct | Line |
| struct | SurveyHybridPrivate |
Macros | |
| #define | SURVEY_HYBRID_APPROACHING_TIME 3.f |
| #define | SURVEY_HYBRID_MAX_POLYGON_SIZE 20 |
| #define | SURVEY_HYBRID_HALF_SWEEP_ENABLED true |
| #define | SURVEY_HYBRID_MAX_SWEEP 0 |
| #define | SURVEY_HYBRID_MAX_SWEEP_BACK 0 |
| #define | SURVEY_HYBRID_ENTRY_DISTANCE (survey_private.sweep_distance / 2.f) |
| #define | SURVEY_HYBRID_ENTRY_CIRCLE TRUE |
| #define | MaxFloat 1000000000 |
| #define | MinFloat -1000000000 |
| #define | LINE_START_FUNCTION {} |
| #define | LINE_STOP_FUNCTION {} |
Enumerations | |
| enum | SurveyStatus { Init , Entry , Sweep , Turn } |
Functions | |
| static void | nav_survey_hybrid_setup (float orientation, float sweep, float radius, float height) |
| finish preparation of survey based on private structure | |
| static void | TranslateAndRotateFromWorld (struct EnuCoor_f *p, float Zrot, struct EnuCoor_f *trans) |
| static void | RotateAndTranslateToWorld (struct EnuCoor_f *p, float Zrot, struct EnuCoor_f *trans) |
| Rotates point round z by -Zrot then translates so (0,0) becomes (transX,transY). | |
| static void | FindInterceptOfTwoLines (float *x, float *y, struct Line L1, struct Line L2) |
| static float | EvaluateLineForX (float y, struct Line L) |
| static float | CrossProductZ (struct EnuCoor_f *p1_start, struct EnuCoor_f *p1_end, struct EnuCoor_f *p2_start, struct EnuCoor_f *p2_end) |
| void | nav_survey_hybrid_init (void) |
| Init function. | |
| void | nav_survey_hybrid_setup_orientation (uint8_t start_wp, float orientation, uint8_t size, float sweep, float radius, float height) |
| Setup polygon survey. | |
| void | nav_survey_hybrid_setup_towards (uint8_t start_wp, uint8_t second_wp, uint8_t size, float sweep, float radius, float height) |
| Setup "dynamic" polygon survey with sweep orientation towards a waypoint. | |
| bool | nav_survey_hybrid_run (void) |
| Run polygon hybrid survey. | |
Variables | |
| struct SurveyHybrid | survey_hybrid |
| static struct SurveyHybridPrivate | survey_private |
This routine will cover the enitre area of any Polygon defined in the flightplan which is a convex polygon.
Definition in file nav_survey_hybrid.c.
| struct Line |
Definition at line 77 of file nav_survey_hybrid.c.
| Data Fields | ||
|---|---|---|
| float | b | |
| float | m | |
| float | x | |
| struct SurveyHybridPrivate |
Definition at line 80 of file nav_survey_hybrid.c.
| Data Fields | ||
|---|---|---|
| struct EnuCoor_f | circle | circle center |
| bool | circle_turns | turns with circles (or lines between points otherwise) |
| struct EnuCoor_f | corners[SURVEY_HYBRID_MAX_POLYGON_SIZE] | corners location |
| float | edge_max_y[SURVEY_HYBRID_MAX_POLYGON_SIZE] | tmp point in rotated frame |
| float | edge_min_y[SURVEY_HYBRID_MAX_POLYGON_SIZE] | tmp point in rotated frame |
| struct Line | edges[SURVEY_HYBRID_MAX_POLYGON_SIZE] | polygon edges |
| struct EnuCoor_f | entry | entry point |
| struct EnuCoor_f | from_wp | tmp point in rotated frame |
| float | max_y | tmp value |
| float | orientation | requested orientation in radians |
| float | radius | turn radius |
| struct EnuCoor_f | segment_from | start of current segment |
| struct EnuCoor_f | segment_to | end of current segment |
| uint8_t | size | size of the polygon |
| struct EnuCoor_f | smallest_corner | tmp point in rotated frame |
| enum SurveyStatus | status | current state |
| float | sweep | oriented sweep distance |
| float | sweep_distance | requested sweep distance |
| struct EnuCoor_f | to_wp | tmp point in rotated frame |
| bool | valid | setup is valid |
| #define LINE_START_FUNCTION {} |
Definition at line 118 of file nav_survey_hybrid.c.
| #define LINE_STOP_FUNCTION {} |
Definition at line 121 of file nav_survey_hybrid.c.
| #define MaxFloat 1000000000 |
Definition at line 114 of file nav_survey_hybrid.c.
Referenced by nav_survey_hybrid_setup(), nav_survey_poly_osam_setup(), and nav_survey_poly_setup().
| #define MinFloat -1000000000 |
Definition at line 115 of file nav_survey_hybrid.c.
| #define SURVEY_HYBRID_APPROACHING_TIME 3.f |
Definition at line 44 of file nav_survey_hybrid.c.
Referenced by nav_survey_hybrid_run().
| #define SURVEY_HYBRID_ENTRY_CIRCLE TRUE |
Definition at line 74 of file nav_survey_hybrid.c.
Referenced by nav_survey_hybrid_run().
| #define SURVEY_HYBRID_ENTRY_DISTANCE (survey_private.sweep_distance / 2.f) |
Definition at line 69 of file nav_survey_hybrid.c.
Referenced by nav_survey_hybrid_setup().
| #define SURVEY_HYBRID_HALF_SWEEP_ENABLED true |
Definition at line 54 of file nav_survey_hybrid.c.
Referenced by nav_survey_hybrid_init().
| #define SURVEY_HYBRID_MAX_POLYGON_SIZE 20 |
Definition at line 49 of file nav_survey_hybrid.c.
Referenced by nav_survey_hybrid_setup_orientation().
| #define SURVEY_HYBRID_MAX_SWEEP 0 |
Definition at line 59 of file nav_survey_hybrid.c.
Referenced by nav_survey_hybrid_init().
| #define SURVEY_HYBRID_MAX_SWEEP_BACK 0 |
Definition at line 64 of file nav_survey_hybrid.c.
Referenced by nav_survey_hybrid_init().
| enum SurveyStatus |
| Enumerator | |
|---|---|
| Init | |
| Entry | |
| Sweep | |
| Turn | |
Definition at line 78 of file nav_survey_hybrid.c.
|
static |
Definition at line 654 of file nav_survey_hybrid.c.
References EnuCoor_f::x, and EnuCoor_f::y.
Referenced by nav_survey_hybrid_run().
|
static |
Definition at line 649 of file nav_survey_hybrid.c.
References Line::b, Line::m, and EnuCoor_f::y.
Referenced by nav_survey_hybrid_run(), and nav_survey_hybrid_setup().
Definition at line 642 of file nav_survey_hybrid.c.
References Line::b, Line::m, EnuCoor_f::x, and EnuCoor_f::y.
Referenced by nav_survey_hybrid_setup().
| void nav_survey_hybrid_init | ( | void | ) |
Init function.
Definition at line 184 of file nav_survey_hybrid.c.
References mission_register(), survey_hybrid, SURVEY_HYBRID_HALF_SWEEP_ENABLED, SURVEY_HYBRID_MAX_SWEEP, SURVEY_HYBRID_MAX_SWEEP_BACK, and survey_private.
| bool nav_survey_hybrid_run | ( | void | ) |
Run polygon hybrid survey.
Definition at line 399 of file nav_survey_hybrid.c.
References CloseRadAngles, CrossProductZ(), dc_autoshoot, DC_AUTOSHOOT_SURVEY, dc_distance_interval, dc_send_command(), DC_SHOOT, dir, Entry, EvaluateLineForX(), Init, LINE_START_FUNCTION, LINE_STOP_FUNCTION, nav, nav_init_stage(), NavCircleCount, RotateAndTranslateToWorld(), stateGetPositionEnu_f(), survey_hybrid, SURVEY_HYBRID_APPROACHING_TIME, SURVEY_HYBRID_ENTRY_CIRCLE, survey_private, Sweep, Turn, VECT3_COPY, EnuCoor_f::x, EnuCoor_f::y, and EnuCoor_f::z.
|
static |
finish preparation of survey based on private structure
Definition at line 202 of file nav_survey_hybrid.c.
References Entry, EvaluateLineForX(), FindInterceptOfTwoLines(), FLOAT_VECT2_ZERO, Init, LINE_STOP_FUNCTION, MaxFloat, NavVerticalAltitudeMode, survey_hybrid, SURVEY_HYBRID_ENTRY_DISTANCE, survey_private, sweep, TranslateAndRotateFromWorld(), and VECT2_COPY.
Referenced by nav_survey_hybrid_setup_orientation().
| void nav_survey_hybrid_setup_orientation | ( | uint8_t | start_wp, |
| float | orientation, | ||
| uint8_t | size, | ||
| float | sweep, | ||
| float | radius, | ||
| float | height ) |
Setup polygon survey.
| start_wp | first waypoint/corner of the polygon |
| orientation | angle of scan lines in degrees (CCW, east) |
| size | number of waypoints/corners used to define the polygon |
| sweep | distance between scan lines |
| radius | turn radius (<0: automatic, radius = sweep/2; 0: no turns, use straight lines only; >0: fixed radius) |
| height | starting height in meters |
Definition at line 365 of file nav_survey_hybrid.c.
References nav_survey_hybrid_setup(), SURVEY_HYBRID_MAX_POLYGON_SIZE, survey_private, sweep, and waypoint_get_enu_f().
Referenced by nav_survey_hybrid_setup_towards().
| void nav_survey_hybrid_setup_towards | ( | uint8_t | start_wp, |
| uint8_t | second_wp, | ||
| uint8_t | size, | ||
| float | sweep, | ||
| float | radius, | ||
| float | height ) |
Setup "dynamic" polygon survey with sweep orientation towards a waypoint.
Computes the sweep orientation angle from the line first-second WP.
| start_wp | first waypoint/corner of the polygon |
| second_wp | second waypoint towards which the sweep orientation is computed |
| size | number of waypoints/corners used to define the polygon |
| sweep | distance between scan lines, if zero uses Poly_Distance |
| radius | turn radius (<0: automatic, radius = sweep/2; 0: no turns, use straight lines only; >0: fixed radius) |
| height | starting height in meters |
Definition at line 383 of file nav_survey_hybrid.c.
References nav_survey_hybrid_setup_orientation(), survey_private, sweep, waypoint_get_enu_f(), EnuCoor_f::x, and EnuCoor_f::y.
|
static |
Rotates point round z by -Zrot then translates so (0,0) becomes (transX,transY).
Definition at line 631 of file nav_survey_hybrid.c.
References p, EnuCoor_f::x, and EnuCoor_f::y.
Referenced by nav_survey_hybrid_run().
|
static |
Definition at line 618 of file nav_survey_hybrid.c.
References p, EnuCoor_f::x, and EnuCoor_f::y.
Referenced by nav_survey_hybrid_setup().
| struct SurveyHybrid survey_hybrid |
Definition at line 103 of file nav_survey_hybrid.c.
Referenced by nav_survey_hybrid_init(), nav_survey_hybrid_run(), and nav_survey_hybrid_setup().
|
static |
Definition at line 104 of file nav_survey_hybrid.c.
Referenced by nav_survey_hybrid_init(), nav_survey_hybrid_run(), nav_survey_hybrid_setup(), nav_survey_hybrid_setup_orientation(), and nav_survey_hybrid_setup_towards().