diff --git a/conf/modules/gvf_module.xml b/conf/modules/gvf_module.xml index 25428f69a6..80b55dc5c4 100644 --- a/conf/modules/gvf_module.xml +++ b/conf/modules/gvf_module.xml @@ -35,6 +35,7 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_ + diff --git a/conf/modules/nav_basic_fw.xml b/conf/modules/nav_basic_fw.xml index 39241566d2..cfc4883124 100644 --- a/conf/modules/nav_basic_fw.xml +++ b/conf/modules/nav_basic_fw.xml @@ -21,7 +21,7 @@ - + diff --git a/sw/airborne/firmwares/fixedwing/nav.c b/sw/airborne/firmwares/fixedwing/nav.c index 8e5be0f010..1b0711d86e 100644 --- a/sw/airborne/firmwares/fixedwing/nav.c +++ b/sw/airborne/firmwares/fixedwing/nav.c @@ -54,7 +54,7 @@ float carrot_x, carrot_y; float nav_circle_radians; /* Cumulated */ float nav_circle_radians_no_rewind; /* Cumulated */ float nav_circle_trigo_qdr; /* Angle from center to mobile */ -float nav_radius, nav_course, nav_climb, nav_shift, sweep_var; +float nav_radius, nav_course, nav_climb, nav_shift; /** Status on the current leg (percentage, 0. < < 1.) in route mode */ diff --git a/sw/airborne/firmwares/fixedwing/nav.h b/sw/airborne/firmwares/fixedwing/nav.h index d916f0783e..9aef725e9d 100644 --- a/sw/airborne/firmwares/fixedwing/nav.h +++ b/sw/airborne/firmwares/fixedwing/nav.h @@ -107,7 +107,6 @@ extern float nav_radius; /* m */ extern float nav_course; /* degrees, clockwise, 0.0 = N */ extern float nav_climb; /* m/s */ extern float nav_shift; /* Lateral shift along a route. In meters */ -extern float sweep_var; //added to allow dynamic sweep width in surveys extern float nav_ground_speed_pgain, nav_ground_speed_setpoint; diff --git a/sw/airborne/modules/guidance/gvf/nav/nav_survey_polygon_gvf.c b/sw/airborne/modules/guidance/gvf/nav/nav_survey_polygon_gvf.c index 18ba21f53c..3f910f6d86 100644 --- a/sw/airborne/modules/guidance/gvf/nav/nav_survey_polygon_gvf.c +++ b/sw/airborne/modules/guidance/gvf/nav/nav_survey_polygon_gvf.c @@ -39,6 +39,8 @@ #include "modules/digital_cam/dc.h" #endif +float gvf_nav_survey_sweep = 100.f; // dummy non-zero value, will be set at setup + struct gvf_SurveyPolyAdv gvf_survey; static void gvf_nav_points(struct FloatVect2 start, struct FloatVect2 end) @@ -147,6 +149,7 @@ void gvf_nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, f gvf_survey.poly_first = first_wp; gvf_survey.poly_count = size; + gvf_nav_survey_sweep = sweep_width; gvf_survey.psa_sweep_width = sweep_width; gvf_survey.psa_min_rad = min_rad; gvf_survey.psa_shot_dist = shot_dist; @@ -247,7 +250,7 @@ void gvf_nav_direction_circle(float rad) bool gvf_nav_survey_polygon_run(void) { #ifdef NAV_SURVEY_POLY_GVF_DYNAMIC - sweep_width = (nav_survey_shift > 0 ? sweep_var : -sweep_var); + sweep_width = (nav_survey_shift > 0 ? gvf_nav_survey_sweep : -gvf_nav_survey_sweep); #endif NavVerticalAutoThrottleMode(0.0); diff --git a/sw/airborne/modules/guidance/gvf/nav/nav_survey_polygon_gvf.h b/sw/airborne/modules/guidance/gvf/nav/nav_survey_polygon_gvf.h index ac36c275c7..d554b63035 100644 --- a/sw/airborne/modules/guidance/gvf/nav/nav_survey_polygon_gvf.h +++ b/sw/airborne/modules/guidance/gvf/nav/nav_survey_polygon_gvf.h @@ -83,6 +83,9 @@ struct gvf_SurveyPolyAdv { struct FloatVect2 ret_end; }; +// external setting +extern float gvf_nav_survey_sweep; + extern void gvf_nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist, float min_rad, float altitude); diff --git a/sw/airborne/modules/nav/nav_survey_poly_osam.c b/sw/airborne/modules/nav/nav_survey_poly_osam.c index 0da5fcd905..4944939682 100644 --- a/sw/airborne/modules/nav/nav_survey_poly_osam.c +++ b/sw/airborne/modules/nav/nav_survey_poly_osam.c @@ -178,7 +178,7 @@ void nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float O SurveyEntryWP = EntryWP; SurveySize = Size; - sweep_var = sw; + Poly_Sweep = sw; struct Point2D Corners[PolygonSize]; @@ -349,9 +349,9 @@ void nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float O bool nav_survey_poly_osam_run(void) { #ifdef NAV_SURVEY_POLY_OSAM_DYNAMIC - dSweep = (nav_survey_shift > 0 ? sweep_var : -sweep_var); + dSweep = (nav_survey_shift > 0 ? Poly_Sweep : -Poly_Sweep); #endif - + struct Point2D C; struct Point2D ToP; struct Point2D FromP; diff --git a/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.c b/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.c index 519b449392..ff6ae34137 100644 --- a/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.c +++ b/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.c @@ -59,7 +59,6 @@ uint8_t Poly_Size = POLYSURVEY_DEFAULT_SIZE; float Poly_Distance = POLYSURVEY_DEFAULT_DISTANCE; -float sweep_var; void nav_survey_poly_setup_towards(uint8_t FirstWP, uint8_t Size, float Sweep, int SecondWP) { @@ -149,7 +148,7 @@ void nav_survey_poly_setup(uint8_t EntryWP, uint8_t Size, float sw, float Orient SurveyEntryWP = EntryWP; SurveySize = Size; - sweep_var = sw; + Poly_Distance = sw; struct EnuCoor_f Corners[MaxPolygonSize]; @@ -309,8 +308,8 @@ bool nav_survey_poly_run(void) { #ifdef NAV_SURVEY_POLY_DYNAMIC - dSweep = (nav_survey_shift > 0 ? sweep_var : -sweep_var); - #endif + dSweep = (nav_survey_shift > 0 ? Poly_Distance : -Poly_Distance); + #endif struct EnuCoor_f C; struct EnuCoor_f ToP; diff --git a/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.h b/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.h index ef6a3919d5..41566816bf 100644 --- a/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.h +++ b/sw/airborne/modules/nav/nav_survey_poly_rotorcraft.h @@ -31,7 +31,6 @@ extern uint8_t Poly_Size; extern float Poly_Distance; -extern float sweep_var; extern uint16_t PolySurveySweepNum; extern uint16_t PolySurveySweepBackNum; extern bool Half_Sweep_Enabled; @@ -49,7 +48,7 @@ extern void nav_survey_poly_setup(uint8_t FirstWP, uint8_t Size, float Sweep, fl * 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). + * Poly_Distance 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, * if zero uses Poly_Size diff --git a/sw/airborne/subsystems/navigation/nav_survey_rectangle.c b/sw/airborne/subsystems/navigation/nav_survey_rectangle.c index ce111ebd00..67ab54f334 100644 --- a/sw/airborne/subsystems/navigation/nav_survey_rectangle.c +++ b/sw/airborne/subsystems/navigation/nav_survey_rectangle.c @@ -30,6 +30,8 @@ #include "subsystems/navigation/nav_survey_rectangle.h" #include "state.h" +float nav_survey_sweep; + static struct point survey_from; static struct point survey_to; static bool survey_uturn __attribute__((unused)) = false; @@ -82,7 +84,7 @@ void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orie } } nav_survey_shift = grid; - sweep_var = grid; + nav_survey_sweep = grid; survey_uturn = false; LINE_START_FUNCTION; } @@ -90,7 +92,7 @@ void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orie void nav_survey_rectangle(uint8_t wp1, uint8_t wp2) { #ifdef NAV_SURVEY_RECTANGLE_DYNAMIC - nav_survey_shift = (nav_survey_shift > 0 ? sweep_var : -sweep_var); + nav_survey_shift = (nav_survey_shift > 0 ? nav_survey_sweep : -nav_survey_sweep); #endif static float survey_radius; diff --git a/sw/airborne/subsystems/navigation/nav_survey_rectangle.h b/sw/airborne/subsystems/navigation/nav_survey_rectangle.h index f563f8c6a5..65400fc55d 100644 --- a/sw/airborne/subsystems/navigation/nav_survey_rectangle.h +++ b/sw/airborne/subsystems/navigation/nav_survey_rectangle.h @@ -33,7 +33,7 @@ #include "firmwares/fixedwing/nav.h" typedef enum {NS, WE} survey_orientation_t; -extern float sweep_var; //added to allow dynamic grid argument +extern float nav_survey_sweep; //added to allow dynamic grid argument extern void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orientation_t so); extern void nav_survey_rectangle(uint8_t wp1, uint8_t wp2);