diff --git a/conf/modules/nav_survey_hybrid.xml b/conf/modules/nav_survey_hybrid.xml index 55d799a5ed..1cfd5eb35b 100644 --- a/conf/modules/nav_survey_hybrid.xml +++ b/conf/modules/nav_survey_hybrid.xml @@ -11,11 +11,11 @@ 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 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 - radius can be: negative, automatically set to sweep/2; zero, not turning on circles; positive, fixed radius - 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).
@@ -42,6 +42,7 @@
+ diff --git a/sw/airborne/modules/nav/nav_survey_hybrid.c b/sw/airborne/modules/nav/nav_survey_hybrid.c index 8852a4e6c7..473cf1cbb1 100644 --- a/sw/airborne/modules/nav/nav_survey_hybrid.c +++ b/sw/airborne/modules/nav/nav_survey_hybrid.c @@ -130,7 +130,7 @@ static bool nav_survey_hybrid_mission_local(uint8_t nb, float *params, enum Miss else { survey_private.size = 4; } for (int i = 0; i < survey_private.size; 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; } 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; } for (int i = 0; i < survey_private.size; i++) { struct LlaCoor_f lla = { - .lat = params[4+2*i], - .lon = params[4+2*i], + .lat = RadOfDeg(params[4+2*i]), + .lon = RadOfDeg(params[4+2*i+1]), .alt = state.ned_origin_f.lla.alt + height }; struct EnuCoor_f corner; @@ -353,6 +353,7 @@ static void nav_survey_hybrid_setup(float orientation, float sweep, float radius LINE_STOP_FUNCTION; 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) @@ -370,7 +371,6 @@ void nav_survey_hybrid_setup_orientation(uint8_t start_wp, float orientation, ui } survey_private.size = size; - survey_private.valid = true; 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) { return; } - survey_private.valid = true; float dx = second->x - start->x; float dy = second->y - start->y;