Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-28 09:58:23 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[nav] fix survey hybrid (#3025)

Browse Source 
* [nav] fix survey hybrid

- call init function
- set to valid at the end of setup so it works with mission correctly

* [nav] fix unit, indexes and doc for survey hybrid
This commit is contained in:
Gautier Hattenberger
2023-04-24 14:51:52 +02:00
committed by GitHub
parent 2a98d9fb69
commit 5f4a4603f0
2 changed files with 7 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/nav_survey_hybrid.xml
+3 -2
View File
@@ -11,11 +11,11 @@
Support mission mode with custom elements: Support mission mode with custom elements:
- points in local NED: SRVHL orientation sweep_distance radius height p1x p1y p2x p2y p3x p3y [p4x p4y] - points in local NED: SRVHL orientation sweep_distance radius height p1x p1y p2x p2y p3x p3y [p4x p4y]
- points in global LLA: SRVHG orientation sweep_distance radius height p1lat p1lon p2lat p2lon p3lat p3lon [p4lat p4lon] - points in global LLA: SRVHG orientation sweep_distance radius height p1lat p1lon p2lat p2lon p3lat p3lon [p4lat p4lon]
- orientation is in degrees - orientation is in degrees, 0 pointing at east, positive counter clockwise
- sweep_distance in meters - sweep_distance in meters
- radius can be: negative, automatically set to sweep/2; zero, not turning on circles; positive, fixed radius - radius can be: negative, automatically set to sweep/2; zero, not turning on circles; positive, fixed radius
- height in meters above reference point - height in meters above reference point
- the polygon in mission mode can have either 3 or 4 points. - the polygon in mission mode can have either 3 or 4 points, with positions in meters (local) or degrees (global).
</description> </description>
<section name="SURVEY_HYBRID" prefix="SURVEY_HYBRID_"> <section name="SURVEY_HYBRID" prefix="SURVEY_HYBRID_">
<define name="MAX_POLYGON_SIZE" value="20" description="max waypoints usable in polygon survey"/> <define name="MAX_POLYGON_SIZE" value="20" description="max waypoints usable in polygon survey"/>
@@ -42,6 +42,7 @@
<header> <header>
<file name="nav_survey_hybrid.h"/> <file name="nav_survey_hybrid.h"/>
</header> </header>
<init fun="nav_survey_hybrid_init()"/>
<makefile target="ap|nps"> <makefile target="ap|nps">
<file name="nav_survey_hybrid.c"/> <file name="nav_survey_hybrid.c"/>
<test firmware="rotorcraft"> <test firmware="rotorcraft">
sw/airborne/modules/nav/nav_survey_hybrid.c
+4 -5
View File
@@ -130,7 +130,7 @@ static bool nav_survey_hybrid_mission_local(uint8_t nb, float *params, enum Miss
else { survey_private.size = 4; } else { survey_private.size = 4; }
for (int i = 0; i < survey_private.size; i++) { for (int i = 0; i < survey_private.size; i++) {
survey_private.corners[i].x = params[4+2*i]; survey_private.corners[i].x = params[4+2*i];
survey_private.corners[i].y = params[5+2*i]; survey_private.corners[i].y = params[5+2*i+1];
survey_private.corners[i].z = height; survey_private.corners[i].z = height;
} }
nav_survey_hybrid_setup(orientation, sweep, radius); nav_survey_hybrid_setup(orientation, sweep, radius);
@@ -155,8 +155,8 @@ static bool nav_survey_hybrid_mission_global(uint8_t nb, float *params, enum Mis
else { survey_private.size = 4; } else { survey_private.size = 4; }
for (int i = 0; i < survey_private.size; i++) { for (int i = 0; i < survey_private.size; i++) {
struct LlaCoor_f lla = { struct LlaCoor_f lla = {
.lat = params[4+2*i], .lat = RadOfDeg(params[4+2*i]),
.lon = params[4+2*i], .lon = RadOfDeg(params[4+2*i+1]),
.alt = state.ned_origin_f.lla.alt + height .alt = state.ned_origin_f.lla.alt + height
}; };
struct EnuCoor_f corner; struct EnuCoor_f corner;
@@ -353,6 +353,7 @@ static void nav_survey_hybrid_setup(float orientation, float sweep, float radius
LINE_STOP_FUNCTION; LINE_STOP_FUNCTION;
NavVerticalAltitudeMode(survey_private.entry.z, 0.f); NavVerticalAltitudeMode(survey_private.entry.z, 0.f);
survey_private.valid = true;
} }
void nav_survey_hybrid_setup_orientation(uint8_t start_wp, float orientation, uint8_t size, float sweep, float radius) void nav_survey_hybrid_setup_orientation(uint8_t start_wp, float orientation, uint8_t size, float sweep, float radius)
@@ -370,7 +371,6 @@ void nav_survey_hybrid_setup_orientation(uint8_t start_wp, float orientation, ui
} }
survey_private.size = size; survey_private.size = size;
survey_private.valid = true;
nav_survey_hybrid_setup(orientation, sweep, radius); nav_survey_hybrid_setup(orientation, sweep, radius);
} }
@@ -382,7 +382,6 @@ void nav_survey_hybrid_setup_towards(uint8_t start_wp, uint8_t second_wp, uint8_
if (start == NULL || second == NULL) { if (start == NULL || second == NULL) {
return; return;
} }
survey_private.valid = true;
float dx = second->x - start->x; float dx = second->x - start->x;
float dy = second->y - start->y; float dy = second->y - start->y;