Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-01 04:46:51 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[nav] exceeding_time on segment

Browse Source 
Add an 'exceeding_time' attribute to 'go' and 'path' blocks that tells how
many seconds the plane should fly after the end of the segment before
completing it.
It is equivalent to giving a negative 'approaching_time' (and these two
attributes are of course not compatible with each other).
In addition some math functions in nav are forced to float.
This commit is contained in:
Gautier Hattenberger
2014-04-01 17:55:29 +02:00
parent 7d917974a4
commit 524205cbf8
3 changed files with 53 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/flight_plans/flight_plan.dtd
+2
View File
@@ -141,6 +141,7 @@ pitch CDATA #IMPLIED
alt CDATA #IMPLIED
height CDATA #IMPLIED
approaching_time CDATA #IMPLIED
exceeding_time CDATA #IMPLIED
throttle CDATA #IMPLIED
climb CDATA #IMPLIED
until CDATA #IMPLIED>
@@ -151,6 +152,7 @@ vmode CDATA #IMPLIED
pitch CDATA #IMPLIED
alt CDATA #IMPLIED
approaching_time CDATA #IMPLIED
exceeding_time CDATA #IMPLIED
throttle CDATA #IMPLIED
climb CDATA #IMPLIED>
sw/airborne/subsystems/nav.c
+43 -29
View File
@@ -109,7 +109,7 @@ void nav_init_stage( void ) {
void nav_circle_XY(float x, float y, float radius) {
struct EnuCoor_f* pos = stateGetPositionEnu_f();
float last_trigo_qdr = nav_circle_trigo_qdr;
nav_circle_trigo_qdr = atan2(pos->y - y, pos->x - x);
nav_circle_trigo_qdr = atan2f(pos->y - y, pos->x - x);
float sign_radius = radius > 0 ? 1 : -1;
if (nav_in_circle) {
@@ -133,7 +133,7 @@ void nav_circle_XY(float x, float y, float radius) {
(dist2_center > Square(abs_radius + dist_carrot)
|| dist2_center < Square(abs_radius - dist_carrot)) ?
0 :
atan((*stateGetHorizontalSpeedNorm_f())*(*stateGetHorizontalSpeedNorm_f()) / (NAV_GRAVITY*radius));
atanf((*stateGetHorizontalSpeedNorm_f())*(*stateGetHorizontalSpeedNorm_f()) / (NAV_GRAVITY*radius));
float carrot_angle = dist_carrot / abs_radius;
carrot_angle = Min(carrot_angle, M_PI/4);
@@ -142,10 +142,10 @@ void nav_circle_XY(float x, float y, float radius) {
horizontal_mode = HORIZONTAL_MODE_CIRCLE;
float radius_carrot = abs_radius;
if (nav_mode == NAV_MODE_COURSE) {
radius_carrot += (abs_radius / cos(carrot_angle) - abs_radius);
radius_carrot += (abs_radius / cosf(carrot_angle) - abs_radius);
}
fly_to_xy(x+cos(alpha_carrot)*radius_carrot,
y+sin(alpha_carrot)*radius_carrot);
fly_to_xy(x+cosf(alpha_carrot)*radius_carrot,
y+sinf(alpha_carrot)*radius_carrot);
nav_in_circle = TRUE;
nav_circle_x = x;
nav_circle_y = y;
@@ -226,14 +226,14 @@ static inline bool_t nav_compute_baseleg(uint8_t wp_af, uint8_t wp_td, uint8_t w
float y_0 = waypoints[wp_td].y - waypoints[wp_af].y;
/* Unit vector from AF to TD */
float d = sqrt(x_0*x_0+y_0*y_0);
float d = sqrtf(x_0*x_0+y_0*y_0);
float x_1 = x_0 / d;
float y_1 = y_0 / d;
waypoints[wp_baseleg].x = waypoints[wp_af].x + y_1 * nav_radius;
waypoints[wp_baseleg].y = waypoints[wp_af].y - x_1 * nav_radius;
waypoints[wp_baseleg].a = waypoints[wp_af].a;
baseleg_out_qdr = M_PI - atan2(-y_1, -x_1);
baseleg_out_qdr = M_PI - atan2f(-y_1, -x_1);
if (nav_radius < 0)
baseleg_out_qdr += M_PI;
@@ -247,7 +247,7 @@ static inline bool_t nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td,
float h_0 = waypoints[wp_td].a - waypoints[wp_af].a;
/* Unit vector from AF to TD */
float d = sqrt(x_0*x_0+y_0*y_0);
float d = sqrtf(x_0*x_0+y_0*y_0);
float x_1 = x_0 / d;
float y_1 = y_0 / d;
@@ -266,8 +266,8 @@ static inline bool_t compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float g
struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
float td_af_x = WaypointX(_af) - WaypointX(_td);
float td_af_y = WaypointY(_af) - WaypointY(_td);
float td_af = sqrt( td_af_x*td_af_x + td_af_y*td_af_y);
float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / glide_vspeed * (glide_airspeed - sqrt(wind->x*wind->x + wind->y*wind->y));
float td_af = sqrtf( td_af_x*td_af_x + td_af_y*td_af_y);
float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / glide_vspeed * (glide_airspeed - sqrtf(wind->x*wind->x + wind->y*wind->y));
WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod;
WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod;
WaypointAlt(_tod) = WaypointAlt(_af);
@@ -292,7 +292,7 @@ static inline void nav_follow(uint8_t _ac_id, float _distance, float _height) {
NavVerticalAutoThrottleMode(0.);
NavVerticalAltitudeMode(Max(ac->alt + _height, ground_alt+SECURITY_HEIGHT), 0.);
float alpha = M_PI/2 - ac->course;
float ca = cos(alpha), sa = sin(alpha);
float ca = cosf(alpha), sa = sinf(alpha);
float x = ac->east - _distance*ca;
float y = ac->north - _distance*sa;
fly_to_xy(x, y);
@@ -314,7 +314,10 @@ float fp_pitch; /* deg */
* Computes \a dist2_to_wp and compare it to square \a carrot.
* Return true if it is smaller. Else computes by scalar products if
* uav has not gone past waypoint.
* Return true if it is the case.
* \a approaching_time can be negative and in this case, the UAV will
* fly after the waypoint for the given number of seconds.
*
* @return true if the position (x, y) is reached
*/
bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time) {
/** distance to waypoint in x */
@@ -322,14 +325,25 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap
/** distance to waypoint in y */
float pw_y = y - stateGetPositionEnu_f()->y;
dist2_to_wp = pw_x*pw_x + pw_y *pw_y;
float min_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f());
if (dist2_to_wp < min_dist*min_dist)
return TRUE;
float scal_prod = (x - from_x) * pw_x + (y - from_y) * pw_y;
return (scal_prod < 0.);
if (approaching_time < 0.) {
// fly after the destination waypoint
float leg_x = x - from_x;
float leg_y = y - from_y;
float leg = sqrtf(Max(leg_x * leg_x + leg_y * leg_y, 1.));
float exceed_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f()); // negative value
float scal_prod = (leg_x * pw_x + leg_y * pw_y) / leg;
return (scal_prod < exceed_dist);
}
else {
// fly close enough of the waypoint or cross it
dist2_to_wp = pw_x*pw_x + pw_y *pw_y;
float min_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f());
if (dist2_to_wp < min_dist*min_dist) {
return TRUE;
}
float scal_prod = (x - from_x) * pw_x + (y - from_y) * pw_y;
return (scal_prod < 0.);
}
}
@@ -342,17 +356,17 @@ void fly_to_xy(float x, float y) {
desired_x = x;
desired_y = y;
if (nav_mode == NAV_MODE_COURSE) {
h_ctl_course_setpoint = atan2(x - pos->x, y - pos->y);
h_ctl_course_setpoint = atan2f(x - pos->x, y - pos->y);
if (h_ctl_course_setpoint < 0.)
h_ctl_course_setpoint += 2 * M_PI;
lateral_mode = LATERAL_MODE_COURSE;
} else {
float diff = atan2(x - pos->x, y - pos->y) - (*stateGetHorizontalSpeedDir_f());
float diff = atan2f(x - pos->x, y - pos->y) - (*stateGetHorizontalSpeedDir_f());
NormRadAngle(diff);
BoundAbs(diff,M_PI/2.);
float s = sin(diff);
float s = sinf(diff);
float speed = *stateGetHorizontalSpeedNorm_f();
h_ctl_roll_setpoint = atan(2 * speed * speed * s * h_ctl_course_pgain / (CARROT * NOMINAL_AIRSPEED * 9.81) );
h_ctl_roll_setpoint = atanf(2 * speed * speed * s * h_ctl_course_pgain / (CARROT * NOMINAL_AIRSPEED * 9.81) );
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
lateral_mode = LATERAL_MODE_ROLL;
}
@@ -366,7 +380,7 @@ 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 = ((stateGetPositionEnu_f()->x - last_wp_x) * leg_x + (stateGetPositionEnu_f()->y - last_wp_y) * leg_y) / leg2;
nav_leg_length = sqrt(leg2);
nav_leg_length = sqrtf(leg2);
/** distance of carrot (in meter) */
float carrot = CARROT * NOMINAL_AIRSPEED;
@@ -548,7 +562,7 @@ void nav_eight(uint8_t target, uint8_t c1, float radius) {
float target_c1_x = waypoints[c1].x - waypoints[target].x;
float target_c1_y = waypoints[c1].y - waypoints[target].y;
float d = sqrt(target_c1_x*target_c1_x+target_c1_y*target_c1_y);
float d = sqrtf(target_c1_x*target_c1_x+target_c1_y*target_c1_y);
d = Max(d, 1.); /* To prevent a division by zero */
/* Unit vector from target to c1 */
@@ -586,7 +600,7 @@ void nav_eight(uint8_t target, uint8_t c1, float radius) {
c2.y - radius * u_x,
alt };
float qdr_out = M_PI - atan2(u_y, u_x);
float qdr_out = M_PI - atan2f(u_y, u_x);
if (radius < 0)
qdr_out += M_PI;
@@ -670,7 +684,7 @@ void nav_oval(uint8_t p1, uint8_t p2, float radius) {
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);
float d = sqrtf(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;
@@ -691,7 +705,7 @@ void nav_oval(uint8_t p1, uint8_t p2, float radius) {
waypoints[p2].y + radius * u_x,
alt };
float qdr_out_2 = M_PI - atan2(u_y, u_x);
float qdr_out_2 = M_PI - atan2f(u_y, u_x);
float qdr_out_1 = qdr_out_2 + M_PI;
if (radius < 0) {
qdr_out_2 += M_PI;
sw/tools/generators/gen_flight_plan.ml
+8 -1
View File
@@ -363,7 +363,14 @@ let rec print_stage = fun index_of_waypoints x ->
lprintf "waypoints[0].y = %s;\n" (parsed_attrib x "y");
"0"
in
let at = try ExtXml.attrib x "approaching_time" with _ -> "CARROT" in
let at = try Some (ExtXml.attrib x "approaching_time") with _ -> None in
let et = try Some (ExtXml.attrib x "exceeding_time") with _ -> None in
let at = match at, et with
| Some a, None -> a
| None, Some e -> "-"^e
| None, None -> "CARROT"
| _, _ -> failwith "Error: 'approaching_time' and 'exceeding_time' attributes are not compatible"
in
let last_wp =
try
get_index_waypoint (ExtXml.attrib x "from") index_of_waypoints