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 28 Packages Projects Releases Wiki Activity

lateral shift along routes

Browse Source
This commit is contained in:
Pascal Brisset
2007-09-10 13:26:39 +00:00
parent b1f9611365
commit 5a9ab6c341
3 changed files with 11 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/settings/tuning.xml
+5
View File
@@ -83,6 +83,11 @@
<dl_setting MAX="1" MIN="0.02" STEP="0.01" VAR="h_ctl_roll_slew" shortname="roll slew"/>
<dl_setting MAX="500" MIN="-500" STEP="5" VAR="nav_radius"/>
<dl_setting MAX="359" MIN="0" STEP="5" VAR="nav_course"/>
<dl_setting MAX="50" MIN="-50" STEP="5" VAR="nav_shift" module="nav" handler="IncreaseShift" shortname="inc. shift">
<strip_button name="<" value="-5"/>
<strip_button name="|" value="0"/>
<strip_button name=">" value="5"/>
</dl_setting>
<dl_setting MAX="20" MIN="10" STEP="0.5" VAR="nav_ground_speed_setpoint" shortname="ground speed"/>
<dl_setting MAX="0." MIN="-0.05" STEP="0.01" VAR="nav_ground_speed_pgain" shortname="ground speed pgain"/>
<dl_setting MAX="500" MIN="50" STEP="5" VAR="nav_survey_shift"/>
sw/airborne/nav.c
+3 -3
View File
@@ -59,7 +59,7 @@ float carrot_x, carrot_y;
/** Status on the current circle */
float nav_circle_radians; /* Cumulated */
float nav_circle_trigo_qdr; /* Angle from center to mobile */
float nav_radius, nav_course;
float nav_radius, nav_course, nav_shift;
/** Status on the current leg (percentage, 0. < < 1.) in route mode */
@@ -95,6 +95,7 @@ void nav_init_stage( void ) {
nav_circle_radians = 0;
nav_in_circle = FALSE;
nav_in_segment = FALSE;
nav_shift = 0;
}
#define PowerVoltage() (vsupply/10.)
@@ -379,7 +380,6 @@ void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) {
float leg_y = wp_y - last_wp_y;
float leg2 = Max(leg_x * leg_x + leg_y * leg_y, 1.);
nav_leg_progress = ((estimator_x - last_wp_x) * leg_x + (estimator_y - last_wp_y) * leg_y) / leg2;
// nav_leg_progress = Max(nav_leg_progress, 0.);
nav_leg_length = sqrt(leg2);
/** distance of carrot (in meter) */
@@ -393,7 +393,7 @@ void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) {
nav_segment_y_2 = wp_y;
horizontal_mode = HORIZONTAL_MODE_ROUTE;
fly_to_xy(last_wp_x + nav_leg_progress*leg_x, last_wp_y + nav_leg_progress*leg_y);
fly_to_xy(last_wp_x + nav_leg_progress*leg_x +nav_shift*leg_y/nav_leg_length, last_wp_y + nav_leg_progress*leg_y-nav_shift*leg_x/nav_leg_length);
}
sw/airborne/nav.h
+3
View File
@@ -112,6 +112,7 @@ extern float ground_alt;
extern float nav_radius; /* m */
extern float nav_course; /* degrees, clockwise, 0.0 = N */
extern float nav_shift; /* Lateral shift along a route. In meters */
extern float nav_ground_speed_pgain, nav_ground_speed_setpoint;
@@ -206,5 +207,7 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap
h_ctl_roll_setpoint = _roll; \
}
#define nav_IncreaseShift(x) { if (x==0) nav_shift = 0; else nav_shift += x; }
#endif /* NAV_H */