diff --git a/conf/flight_plans/IS.xml b/conf/flight_plans/IS.xml index 7c10ddf120..6977dad627 100644 --- a/conf/flight_plans/IS.xml +++ b/conf/flight_plans/IS.xml @@ -66,7 +66,7 @@ - + diff --git a/sw/airborne/discsurvey.c b/sw/airborne/discsurvey.c index 37204714e1..6b5420eadc 100644 --- a/sw/airborne/discsurvey.c +++ b/sw/airborne/discsurvey.c @@ -60,7 +60,7 @@ bool_t disc_survey( uint8_t center, float radius, float grid ) { case SEGMENT: nav_route_xy(c1.x, c1.y, c2.x, c2.y); - if (nav_approaching_xy(c2.x, c2.y, CARROT)) { + if (nav_approaching_xy(c2.x, c2.y, c1.x, c1.y, CARROT)) { c.x = c2.x + grid*upwind_x; c.y = c2.y + grid*upwind_y; diff --git a/sw/airborne/nav.c b/sw/airborne/nav.c index 77e9c3876c..cc837e7eae 100644 --- a/sw/airborne/nav.c +++ b/sw/airborne/nav.c @@ -45,7 +45,7 @@ uint8_t nav_stage, nav_block; /** To save the current block/stage to enable return */ static uint8_t last_block, last_stage; -static float last_x, last_y; +float last_x, last_y; /** Index of last waypoint. Used only in "go" stage in "route" horiz mode */ static uint8_t last_wp __attribute__ ((unused)); @@ -367,7 +367,7 @@ struct point waypoints[NB_WAYPOINT+1] = WAYPOINTS; * uav has not gone past waypoint. * Return true if it is the case. */ -bool_t nav_approaching_xy(float x, float y, float approaching_time) { +bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time) { /** distance to waypoint in x */ float pw_x = x - estimator_x; /** distance to waypoint in y */ @@ -378,7 +378,7 @@ bool_t nav_approaching_xy(float x, float y, float approaching_time) { if (dist2_to_wp < min_dist*min_dist) return TRUE; - float scal_prod = (x - last_x) * pw_x + (y - last_y) * pw_y; + float scal_prod = (x - from_x) * pw_x + (y - from_y) * pw_y; return (scal_prod < 0.); } @@ -573,7 +573,7 @@ void nav_eight(uint8_t target, uint8_t c1, float radius) { case R12: nav_route_xy(c1_out.x, c1_out.y, c2_in.x, c2_in.y); - if (nav_approaching_xy(c2_in.x, c2_in.y,CARROT)) { + if (nav_approaching_xy(c2_in.x, c2_in.y, c1_out.x, c1_out.y, CARROT)) { eight_status = C2; InitStage(); } @@ -589,7 +589,7 @@ void nav_eight(uint8_t target, uint8_t c1, float radius) { case R21: nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y); - if (nav_approaching_xy(c1_in.x, c1_in.y,CARROT)) { + if (nav_approaching_xy(c1_in.x, c1_in.y, c2_out.x, c2_out.y, CARROT)) { eight_status = C1; InitStage(); } @@ -656,7 +656,7 @@ void nav_oval(uint8_t p1, uint8_t p2, float radius) { case OR12: nav_route_xy(p1_out.x, p1_out.y, p2_in.x, p2_in.y); - if (nav_approaching_xy(p2_in.x, p2_in.y,CARROT)) { + if (nav_approaching_xy(p2_in.x, p2_in.y, p1_out.x, p1_out.y, CARROT)) { oval_status = OC2; InitStage(); } @@ -672,7 +672,7 @@ void nav_oval(uint8_t p1, uint8_t p2, float radius) { case OR21: nav_route_xy(waypoints[p2].x, waypoints[p2].y, waypoints[p1].x, waypoints[p1].y); - if (nav_approaching_xy(waypoints[p1].x, waypoints[p1].y,CARROT)) { + if (nav_approaching_xy(waypoints[p1].x, waypoints[p1].y, waypoints[p2].x, waypoints[p2].y, CARROT)) { oval_status = OC1; InitStage(); } diff --git a/sw/airborne/nav.h b/sw/airborne/nav.h index 5d12eb5adf..6797eca863 100644 --- a/sw/airborne/nav.h +++ b/sw/airborne/nav.h @@ -53,6 +53,7 @@ struct point { extern float cur_pos_x; extern float cur_pos_y; +extern float last_x, last_y; extern uint8_t nav_stage, nav_block; extern float dist2_to_wp, dist2_to_home; @@ -146,8 +147,9 @@ extern void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_ #define NavSegment(_start, _end) \ nav_route_xy(waypoints[_start].x, waypoints[_start].y, waypoints[_end].x, waypoints[_end].y) -bool_t nav_approaching_xy(float x, float y, float approaching_time); -#define NavApproaching(wp, time) nav_approaching_xy(waypoints[wp].x, waypoints[wp].y, time) +bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time); +#define NavApproaching(wp, time) nav_approaching_xy(waypoints[wp].x, waypoints[wp].y, last_x, last_y, time) +#define NavApproachingFrom(wp, from, time) nav_approaching_xy(waypoints[wp].x, waypoints[wp].y, waypoints[from].x, waypoints[from].y, time) /** Set the climb control to auto-throttle with the specified pitch pre-command */ diff --git a/sw/simulator/diffusion.ml b/sw/simulator/diffusion.ml index e1dc1feae9..2013e8c266 100644 --- a/sw/simulator/diffusion.ml +++ b/sw/simulator/diffusion.ml @@ -8,7 +8,7 @@ type plume = { mutable utm_x : float; mutable utm_y : float; mutable value : int (* NW of Muret ref *) let muret = utm_of WGS84 {LL.posn_lat=(Deg>>Rad)43.4624; posn_long=(Deg>>Rad)1.2727} -let source = fun () -> { utm_x = muret.LL.utm_x -. 275.; utm_y = muret.LL.utm_y +. 275.; value = 255} +let source = fun () -> { utm_x = muret.LL.utm_x +. 225.; utm_y = muret.LL.utm_y +. 225.; value = 255} let available_ids = ref [] let gen_id = diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index 87647562ca..ad484edee5 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/gen_flight_plan.ml @@ -342,12 +342,15 @@ let rec print_stage = fun index_of_waypoints x -> "0" in let at = try ExtXml.attrib x "approaching_time" with _ -> "CARROT" in - lprintf "if (NavApproaching(%s,%s)) NextStageFrom(%s) else {\n" wp at wp; - right (); let last_wp = try get_index_waypoint (ExtXml.attrib x "from") index_of_waypoints with ExtXml.Error _ -> "last_wp" in + if last_wp = "last_wp" then + lprintf "if (NavApproaching(%s,%s)) NextStageFrom(%s) else {\n" wp at wp + else + lprintf "if (NavApproachingFrom(%s,%s,%s)) NextStageFrom(%s) else {\n" wp last_wp at wp; + right (); let hmode = output_hmode x wp last_wp in let vmode = output_vmode x wp last_wp in if vmode = "glide" && hmode <> "route" then