Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 00:53:41 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

bug fixed in nav route

Browse Source
This commit is contained in:
Pascal Brisset
2007-02-15 12:00:57 +00:00
parent ac826cfd91
commit 32957c3b97
6 changed files with 19 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/flight_plans/IS.xml
+1 -1
View File
@@ -66,7 +66,7 @@
</block>
<block name="Anemotaxis" strip_button="Anem">
<call fun="nav_anemotaxis_downwind(WP_C, 400)"/>
<call fun="nav_anemotaxis_downwind(WP_C, 300)"/>
<go from="HOME" wp="C" hmode="route" approaching_time="0"/>
<call fun="nav_anemotaxis_init(WP_C)"/>
<call fun="nav_anemotaxis(WP_C, WP_C1, WP_C2, WP_PLUME)"/>
sw/airborne/discsurvey.c
+1 -1
View File
@@ -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;
sw/airborne/nav.c
+7 -7
View File
@@ -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();
}
sw/airborne/nav.h
+4 -2
View File
@@ -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 */
sw/simulator/diffusion.ml
+1 -1
View File
@@ -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 =
sw/tools/gen_flight_plan.ml
+5 -2
View File
@@ -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