Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 20:38:27 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

navigation on Ovals (Anton's code)

Browse Source
This commit is contained in:
Pascal Brisset
2006-11-27 13:04:34 +00:00
parent 2bebeef5be
commit 7e0975ee5e
5 changed files with 116 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/flight_plans/flight_plan.dtd
+9 -3
View File
@@ -12,14 +12,14 @@
<!ELEMENT exceptions (exception*)>
<!ELEMENT blocks (block+)>
<!ELEMENT block (exception|while|heading|attitude|go|xyz|set|call|circle|deroute|stay|follow|survey_rectangle|for|return|eight)*>
<!ELEMENT block (exception|while|heading|attitude|go|xyz|set|call|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval)*>
<!ELEMENT include (arg|with)*>
<!ELEMENT arg EMPTY>
<!ELEMENT with EMPTY>
<!ELEMENT while (exception|while|heading|attitude|go|xyz|set|call|circle|deroute|stay|follow|survey_rectangle|for|return|eight)*>
<!ELEMENT for (exception|while|heading|attitude|go|xyz|set|call|circle|deroute|stay|follow|survey_rectangle|for|return|eight)*>
<!ELEMENT while (exception|while|heading|attitude|go|xyz|set|call|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval)*>
<!ELEMENT for (exception|while|heading|attitude|go|xyz|set|call|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval)*>
<!ELEMENT exception EMPTY>
<!ELEMENT heading EMPTY>
<!ELEMENT attitude EMPTY>
@@ -29,6 +29,7 @@
<!ELEMENT call EMPTY>
<!ELEMENT circle EMPTY>
<!ELEMENT eight EMPTY>
<!ELEMENT oval EMPTY>
<!ELEMENT survey_rectangle EMPTY>
<!ELEMENT deroute EMPTY>
<!ELEMENT stay EMPTY>
@@ -155,6 +156,11 @@ center CDATA #REQUIRED
turn_around CDATA #REQUIRED
radius CDATA #REQUIRED>
<!ATTLIST oval
p1 CDATA #REQUIRED
p2 CDATA #REQUIRED
radius CDATA #REQUIRED>
<!ATTLIST survey_rectangle
grid CDATA #REQUIRED
wp1 CDATA #REQUIRED
conf/flight_plans/versatile.xml
+4
View File
@@ -27,6 +27,10 @@
<eight radius="75" center="1" turn_around="2"/>
</block>
<block name="oval" strip_button="Oval">
<oval p1="1" p2="2" radius="80"/>
</block>
<block name="circle 1 auto pitch">
<circle radius="75" wp="1" pitch="auto" throttle="0.7"/>
</block>
sw/airborne/nav.c
+85 -1
View File
@@ -237,7 +237,7 @@ static inline void nav_circle_XY(float x, float y, float radius) {
#define Or(x, y) ((x) || (y))
#define Min(x,y) (x < y ? x : y)
#define Max(x,y) (x > y ? x : y)
#define NavQdrCloseTo(x) ({ float circle_qdr = NavCircleQdr(); (Min(x, 350) < circle_qdr && circle_qdr < x+10); })
#define NavQdrCloseTo(x) ({ float _course = x; NormCourse(_course); float circle_qdr = NavCircleQdr(); (Min(_course, 350) < circle_qdr && circle_qdr < _course+10); })
#define NavBlockTime() (block_time)
#define LessThan(_x, _y) ((_x) < (_y))
@@ -637,3 +637,87 @@ void nav_eight(uint8_t target, uint8_t c1, float radius) {
return;
}
}
/************** Oval Navigation **********************************************/
/** Navigation along a figure O. One side leg is defined by waypoints [p1] and
[p2].
The navigation goes through 4 states: OC1 (half circle next to [p1]),
OR21 (route [p2] to [p1], OC2 (half circle next to [p2]) and OR12
(opposite leg).
Initial state is the route along the desired segment (OC2).
*/
enum oval_status { OR12, OC2, OR21, OC1 };
static enum oval_status oval_status;
void nav_oval_init( void ) {
oval_status = OC2;
}
void nav_oval(uint8_t p1, uint8_t p2, float radius) {
float alt = waypoints[p1].a;
waypoints[p1].a = alt;
float p2_p1_x = waypoints[p1].x - waypoints[p2].x;
float p2_p1_y = waypoints[p1].y - waypoints[p2].y;
float d = sqrt(p2_p1_x*p2_p1_x+p2_p1_y*p2_p1_y);
/* Unit vector from p1 to p2 */
float u_x = p2_p1_x / d;
float u_y = p2_p1_y / d;
/* The half circle centers and the other leg */
struct point p1_center = { waypoints[p1].x + radius * -u_y,
waypoints[p1].y + radius * u_x,
alt };
struct point p1_out = { waypoints[p1].x + 2*radius * -u_y,
waypoints[p1].y + 2*radius * u_x,
alt };
struct point p2_in = { waypoints[p2].x + 2*radius * -u_y,
waypoints[p2].y + 2*radius * u_x,
alt };
struct point p2_center = { waypoints[p2].x + radius * -u_y,
waypoints[p2].y + radius * u_x,
alt };
float qdr_out_2 = M_PI - atan2(u_y, u_x);
float qdr_out_1 = qdr_out_2 + M_PI;
switch (oval_status) {
case OC1 :
nav_circle_XY(p1_center.x,p1_center.y, -radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_1)-10)) {
oval_status = OR12;
InitStage();
}
return;
case OR12:
nav_route_xy(p1_out.x, p1_out.y, p2_in.x, p2_in.y);
if (approaching_xy(p2_in.x, p2_in.y,CARROT)) {
oval_status = OC2;
InitStage();
}
return;
case OC2 :
nav_circle_XY(p2_center.x, p2_center.y, -radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2)-10)) {
oval_status = OR21;
InitStage();
}
return;
case OR21:
nav_route_xy(waypoints[p2].x, waypoints[p2].y, waypoints[p1].x, waypoints[p1].y);
if (approaching_xy(waypoints[p1].x, waypoints[p1].y,CARROT)) {
oval_status = OC1;
InitStage();
}
return;
}
}
sw/airborne/nav.h
+4
View File
@@ -101,6 +101,10 @@ extern void nav_eight_init( void );
extern void nav_eight(uint8_t, uint8_t, float);
#define Eight(a, b, c) nav_eight((a), (b), (c))
extern void nav_oval_init( void );
extern void nav_oval(uint8_t, uint8_t, float);
#define Oval(a, b, c) nav_oval((b), (a), (c))
extern float ground_alt;
extern float survey_west, survey_east, survey_north, survey_south;
sw/tools/gen_flight_plan.ml
+14 -1
View File
@@ -237,7 +237,7 @@ let rec index_stage = fun x ->
| "heading" | "attitude" | "go" | "stay" | "xyz" | "set" | "circle" ->
incr stage;
Xml.Element (Xml.tag x, Xml.attribs x@["no", soi !stage], Xml.children x)
| "survey_rectangle" | "eight" ->
| "survey_rectangle" | "eight" | "oval"->
incr stage; incr stage;
Xml.Element (Xml.tag x, Xml.attribs x@["no", soi !stage], Xml.children x)
| "exception" ->
@@ -404,6 +404,19 @@ let rec print_stage = fun index_of_waypoints x ->
lprintf "Eight(%s, %s, %s);\n" center turn_about r;
output_cam_mode x index_of_waypoints;
lprintf "return;\n"
| "oval" ->
stage ();
lprintf "nav_oval_init();\n";
lprintf "NextStage();\n";
left ();
stage ();
let p1 = get_index_waypoint (ExtXml.attrib x "p1") index_of_waypoints
and p2 = get_index_waypoint (ExtXml.attrib x "p2") index_of_waypoints in
let r = parsed_attrib x "radius" in
let _vmode = output_vmode x p1 "" in
lprintf "Oval(%s, %s, %s);\n" p1 p2 r;
output_cam_mode x index_of_waypoints;
lprintf "return;\n"
| "set" ->
stage ();
let var = ExtXml.attrib x "var"