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

West-East survey

Browse Source
This commit is contained in:
Pascal Brisset
2007-04-23 20:15:49 +00:00
parent 7e3a283f0f
commit da930d5c48
5 changed files with 62 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/flight_plans/biscarosse.xml
+52
View File
@@ -0,0 +1,52 @@
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
<flight_plan alt="250" ground_alt="200" lat0="44.4492" lon0="-1.2555" max_dist_from_home="1000" name="Biscarosse" qfu="270" security_height="25">
<waypoints>
<waypoint name="HOME" x="0" y="0"/>
<waypoint name="1" x="-2.15568649652" y="98.0451414566"/>
<waypoint name="2" x="0" y="200"/>
<waypoint name="S1" x="-613.048818661" y="-76.7757721655"/>
<waypoint name="S2" x="-106.969918801" y="491.90205153"/>
</waypoints>
<blocks>
<block name="wait GPS">
<while cond="!GpsFixValid()"/>
</block>
<block name="init">
<while cond="LessThan(NavBlockTime(), 10)"/>
<call fun="NavSetGroundReferenceHere()"/>
<deroute block="circle 1"/>
</block>
<block name="circle 1">
<circle radius="nav_radius" wp="1"/>
</block>
<block name="eight 1">
<eight center="1" radius="75" turn_around="2"/>
</block>
<block name="oval">
<oval p1="1" p2="2" radius="80"/>
</block>
<block name="circle left 1">
<circle radius="-75" wp="1"/>
</block>
<block name="climb 75">
<circle pitch="0.3" radius="50+(estimator_z-ground_alt)/2" throttle="0.75" until="10 > PowerVoltage()" vmode="throttle" wp="1"/>
</block>
<block name="descent 0">
<circle pitch="-0.3" radius="50+(estimator_z-ground_alt)/2" throttle="0.0" until="ground_alt+50 > estimator_z" vmode="throttle" wp="1"/>
<deroute block="circle 1"/>
</block>
<block name="glide12">
<go from="1" hmode="route" vmode="glide" wp="2"/>
</block>
<block name="stack 2">
<circle radius="75" wp="2"/>
</block>
<block name="survey WE" strip_button="Survey WE">
<survey_rectangle grid="100" orientation="WE" wp1="S1" wp2="S2"/>
</block>
<block name="survey NS" strip_button="Survey NS">
<survey_rectangle grid="100" wp1="S1" wp2="S2"/>
</block>
</blocks>
</flight_plan>
conf/flight_plans/flight_plan.dtd
+1
View File
@@ -163,6 +163,7 @@ radius CDATA #REQUIRED>
<!ATTLIST survey_rectangle <!ATTLIST survey_rectangle
grid CDATA #REQUIRED grid CDATA #REQUIRED
orientation CDATA #IMPLIED
wp1 CDATA #REQUIRED wp1 CDATA #REQUIRED
wp2 CDATA #REQUIRED> wp2 CDATA #REQUIRED>
sw/airborne/nav.c
+2 -98
View File
@@ -56,7 +56,7 @@ float stage_time_ds;
float carrot_x, carrot_y; float carrot_x, carrot_y;
/** Status on the current circle */ /** Status on the current circle */
static float nav_circle_radians; /* Cumulated */ float nav_circle_radians; /* Cumulated */
float nav_circle_trigo_qdr; /* Angle from center to mobile */ float nav_circle_trigo_qdr; /* Angle from center to mobile */
float nav_radius; float nav_radius;
@@ -158,9 +158,6 @@ void nav_circle_XY(float x, float y, float radius) {
fly_to_xy(waypoints[_wp].x, waypoints[_wp].y); \ fly_to_xy(waypoints[_wp].x, waypoints[_wp].y); \
} }
#define NavSurveyRectangleInit(_wp1, _wp2, _grid) nav_survey_rectangle_init(_wp1, _wp2, _grid)
#define NavSurveyRectangle(_wp1, _wp2) nav_survey_rectangle(_wp1, _wp2)
#define NavGlide(_last_wp, _wp) { \ #define NavGlide(_last_wp, _wp) { \
float start_alt = waypoints[_last_wp].a; \ float start_alt = waypoints[_last_wp].a; \
float diff_alt = waypoints[_wp].a - start_alt; \ float diff_alt = waypoints[_wp].a - start_alt; \
@@ -214,83 +211,6 @@ void nav_circle_XY(float x, float y, float radius) {
#define NavSetGroundReferenceHere() ({ nav_reset_reference(); nav_update_waypoints_alt(); FALSE; }) #define NavSetGroundReferenceHere() ({ nav_reset_reference(); nav_update_waypoints_alt(); FALSE; })
/** Automatic survey of a sector (south-north sweep) */
static struct point survey_from;
static struct point survey_to;
float survey_shift;
static bool_t survey_uturn __attribute__ ((unused)) = FALSE;
float survey_west, survey_east, survey_north, survey_south;
#define SurveyGoingNorth() (survey_to.y > survey_from.y)
#define SurveyGoingSouth() (survey_to.y < survey_from.y)
static inline void nav_survey_rectangle(uint8_t wp1, uint8_t wp2) {
static float survey_radius;
survey_west = Min(waypoints[wp1].x, waypoints[wp2].x);
survey_east = Max(waypoints[wp1].x, waypoints[wp2].x);
survey_south = Min(waypoints[wp1].y, waypoints[wp2].y);
survey_north = Max(waypoints[wp1].y, waypoints[wp2].y);
if (SurveyGoingNorth()) {
survey_to.y = survey_north;
survey_from.y = survey_south;
}
if (SurveyGoingSouth()) {
survey_to.y = survey_south;
survey_from.y = survey_north;
}
if (! survey_uturn) { /* S-N or N-S straight route */
if ((estimator_y < survey_north && SurveyGoingNorth()) ||
(estimator_y > survey_south && SurveyGoingSouth())) {
/* Continue ... */
nav_route_xy(survey_from.x, survey_from.y, survey_to.x, survey_to.y);
} else {
/* North or South limit reached, prepare U-turn and next leg */
nav_in_segment = FALSE;
float x0 = survey_from.x; /* Current longitude */
if (x0+survey_shift < survey_west || x0+survey_shift > survey_east) {
x0 += survey_shift / 2;
survey_shift = -survey_shift;
nav_circle_radians = M_PI_2;
} else {
nav_circle_radians = 0.;
}
x0 = x0 + survey_shift; /* Longitude of next leg */
survey_from.x = survey_to.x = x0;
/* Swap South and North extremities */
float tmp = survey_from.y;
survey_from.y = survey_to.y;
survey_to.y = tmp;
/** Do half a circle around WP 0 */
waypoints[0].x = x0 - survey_shift/2.;
waypoints[0].y = survey_from.y;
/* Computes the right direction for the circle */
survey_radius = survey_shift / 2.;
if (SurveyGoingNorth()) {
survey_radius = -survey_radius;
if (survey_radius > 0.)
nav_circle_radians = - nav_circle_radians;
}
survey_uturn = TRUE;
}
} else { /* U-turn */
if (NavCircleCount() < 0.45) {
NavCircleWaypoint(0, survey_radius);
} else {
/* U-turn finished, back on a segment */
survey_uturn = FALSE;
nav_in_circle = FALSE;
}
}
NavVerticalAutoThrottleMode(0.); /* No pitch */
NavVerticalAltitudeMode(WaypointAlt(wp1), 0.); /* No preclimb */
}
void nav_goto_block(uint8_t b) { void nav_goto_block(uint8_t b) {
if (b != nav_block) { /* To avoid a loop in a the current block */ if (b != nav_block) { /* To avoid a loop in a the current block */
@@ -300,22 +220,6 @@ void nav_goto_block(uint8_t b) {
GotoBlock(b); GotoBlock(b);
} }
static inline void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid) {
survey_west = Min(waypoints[wp1].x, waypoints[wp2].x);
survey_east = Max(waypoints[wp1].x, waypoints[wp2].x);
survey_south = Min(waypoints[wp1].y, waypoints[wp2].y);
survey_north = Max(waypoints[wp1].y, waypoints[wp2].y);
survey_from.x = survey_to.x = Min(Max(estimator_x, survey_west+grid/2.), survey_east-grid/2.);
if (estimator_y > survey_north || (estimator_y > survey_south && estimator_hspeed_dir > M_PI/2. && estimator_hspeed_dir < 3*M_PI/2)) {
survey_to.y = survey_south;
survey_from.y = survey_north;
} else {
survey_from.y = survey_south;
survey_to.y = survey_north;
}
survey_shift = grid;
survey_uturn = FALSE;
}
static unit_t unit __attribute__ ((unused)); static unit_t unit __attribute__ ((unused));
@@ -369,7 +273,7 @@ static unit_t nav_reset_reference( void ) {
return 0; return 0;
} }
/** Shift altitude of the waypoint accorgint to a new ground altitude */ /** Shift altitude of the waypoint according to a new ground altitude */
static unit_t nav_update_waypoints_alt( void ) { static unit_t nav_update_waypoints_alt( void ) {
uint8_t i; uint8_t i;
for(i = 0; i <= NB_WAYPOINT; i++) { for(i = 0; i <= NB_WAYPOINT; i++) {
sw/airborne/nav.h
+2 -3
View File
@@ -36,6 +36,7 @@
#include "paparazzi.h" #include "paparazzi.h"
#include "airframe.h" #include "airframe.h"
#include "fw_v_ctl.h" #include "fw_v_ctl.h"
#include "nav_survey_rectangle.h"
#define G 9.806 #define G 9.806
#define Square(_x) ((_x)*(_x)) #define Square(_x) ((_x)*(_x))
@@ -69,7 +70,6 @@ extern float desired_x, desired_y, altitude_shift, nav_altitude, flight_altitude
extern pprz_t nav_throttle_setpoint; extern pprz_t nav_throttle_setpoint;
extern float nav_pitch, rc_pitch; extern float nav_pitch, rc_pitch;
extern bool_t too_far_from_home; extern bool_t too_far_from_home;
extern float survey_shift;
/** in second */ /** in second */
extern uint16_t stage_time, block_time; extern uint16_t stage_time, block_time;
@@ -79,6 +79,7 @@ extern float stage_time_ds;
extern float carrot_x, carrot_y; extern float carrot_x, carrot_y;
extern float nav_circle_radians; /* Cumulated */
extern bool_t nav_in_circle; extern bool_t nav_in_circle;
extern bool_t nav_in_segment; extern bool_t nav_in_segment;
extern int16_t nav_circle_x, nav_circle_y, nav_circle_radius; extern int16_t nav_circle_x, nav_circle_y, nav_circle_radius;
@@ -108,8 +109,6 @@ extern void nav_oval(uint8_t, uint8_t, float);
extern float ground_alt; extern float ground_alt;
extern float survey_west, survey_east, survey_north, survey_south;
extern float nav_radius; extern float nav_radius;
extern float nav_ground_speed_pgain, nav_ground_speed_setpoint; extern float nav_ground_speed_pgain, nav_ground_speed_setpoint;
sw/tools/gen_flight_plan.ml
+5 -2
View File
@@ -435,9 +435,12 @@ let rec print_stage = fun index_of_waypoints x ->
| "survey_rectangle" -> | "survey_rectangle" ->
let grid = parsed_attrib x "grid" let grid = parsed_attrib x "grid"
and wp1 = get_index_waypoint (ExtXml.attrib x "wp1") index_of_waypoints and wp1 = get_index_waypoint (ExtXml.attrib x "wp1") index_of_waypoints
and wp2 = get_index_waypoint (ExtXml.attrib x "wp2") index_of_waypoints in and wp2 = get_index_waypoint (ExtXml.attrib x "wp2") index_of_waypoints
and orientation = ExtXml.attrib_or_default x "orientation" "NS" in
stage (); stage ();
lprintf "NavSurveyRectangleInit(%s, %s, %s);\n" wp1 wp2 grid; if orientation <> "NS" && orientation <> "WE" then
failwith (sprintf "Unknown survey orientation (NS or WE): %s" orientation);
lprintf "NavSurveyRectangleInit(%s, %s, %s, %s);\n" wp1 wp2 grid orientation;
lprintf "NextStage();\n"; lprintf "NextStage();\n";
left (); left ();
stage (); stage ();