From f6a7356ef4775aaf9f8fd5d9b9edca1a6b3544c3 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 21 Mar 2023 22:19:25 +0100 Subject: [PATCH] [nav] implement nav_circle for hybrids The flight speed is adapted to the circle radius according to the maximum bank angle allowed. The oval implemented in base rotorcraft nav is also compatible with the hybrid primitives registered in nav. --- sw/airborne/math/pprz_algebra_float.h | 4 +- sw/airborne/modules/nav/nav_rotorcraft_base.c | 12 ++-- .../modules/nav/nav_rotorcraft_hybrid.c | 65 ++++++++++++++++--- 3 files changed, 65 insertions(+), 16 deletions(-) diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index af6bf3a4442..545955125f9 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -145,7 +145,7 @@ static inline float float_vect2_norm(struct FloatVect2 *v) static inline void float_vect2_normalize(struct FloatVect2 *v) { const float n = float_vect2_norm(v); - if (n > 0) { + if (n > 1e-4f) { v->x /= n; v->y /= n; } @@ -177,7 +177,7 @@ static inline float float_vect3_norm(struct FloatVect3 *v) static inline void float_vect3_normalize(struct FloatVect3 *v) { const float n = float_vect3_norm(v); - if (n > 0) { + if (n > 1e-4f) { v->x /= n; v->y /= n; v->z /= n; diff --git a/sw/airborne/modules/nav/nav_rotorcraft_base.c b/sw/airborne/modules/nav/nav_rotorcraft_base.c index f506a98816e..90b6d23729a 100644 --- a/sw/airborne/modules/nav/nav_rotorcraft_base.c +++ b/sw/airborne/modules/nav/nav_rotorcraft_base.c @@ -215,7 +215,7 @@ static void nav_oval(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius) switch (nav_rotorcraft_base.oval.status) { case OC1 : - nav_circle(&p1_center, radius); + nav.nav_circle(&p1_center, radius); if (NavQdrCloseTo(DegOfRad(qdr_out_1) - qdr_anticipation)) { nav_rotorcraft_base.oval.status = OR12; InitStage(); @@ -224,8 +224,8 @@ static void nav_oval(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius) return; case OR12: - nav_route(&p1_out, &p2_in); - if (nav_approaching(&p2_in, &p1_out, CARROT)) { + nav.nav_route(&p1_out, &p2_in); + if (nav.nav_approaching(&p2_in, &p1_out, CARROT)) { nav_rotorcraft_base.oval.status = OC2; nav_rotorcraft_base.oval.count++; InitStage(); @@ -234,7 +234,7 @@ static void nav_oval(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius) return; case OC2 : - nav_circle(&p2_center, radius); + nav.nav_circle(&p2_center, radius); if (NavQdrCloseTo(DegOfRad(qdr_out_2) - qdr_anticipation)) { nav_rotorcraft_base.oval.status = OR21; InitStage(); @@ -243,8 +243,8 @@ static void nav_oval(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius) return; case OR21: - nav_route(wp2, wp1); - if (nav_approaching(wp1, wp2, CARROT)) { + nav.nav_route(wp2, wp1); + if (nav.nav_approaching(wp1, wp2, CARROT)) { nav_rotorcraft_base.oval.status = OC1; InitStage(); LINE_STOP_FUNCTION; diff --git a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c index 68fbe6a2d4b..958b0df3520 100644 --- a/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c +++ b/sw/airborne/modules/nav/nav_rotorcraft_hybrid.c @@ -26,6 +26,7 @@ #include "modules/nav/nav_rotorcraft_hybrid.h" #include "firmwares/rotorcraft/navigation.h" #include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h" // strong dependency for now +#include "math/pprz_isa.h" // Max ground speed that will be commanded #ifndef GUIDANCE_INDI_NAV_SPEED_MARGIN @@ -48,6 +49,10 @@ static float guidance_indi_line_gain = 1.0f; #define GUIDANCE_INDI_NAV_LINE_DIST 50.f #endif +#ifndef GUIDANCE_INDI_NAV_CIRCLE_DIST +#define GUIDANCE_INDI_NAV_CIRCLE_DIST 40.f +#endif + /** Implement basic nav function for the hybrid case */ @@ -101,7 +106,7 @@ static void nav_hybrid_route(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_en // Calculate length of line segment float length_line = Max(float_vect2_norm(&wp_diff), 0.01f); - //Normal vector to the line, with length of the line + // Normal vector to the line, with length of the line struct FloatVect2 normalv; VECT2_ASSIGN(normalv, -wp_diff.y, wp_diff.x); // Length of normal vector is the same as of the line segment @@ -171,12 +176,58 @@ static bool nav_hybrid_approaching(struct EnuCoor_f *wp, struct EnuCoor_f *from, return false; } -#if 0 // TODO reuse existing patterns for now - -static void nav_circle(struct EnuCoor_f *wp_center, float radius) +static void nav_hybrid_circle(struct EnuCoor_f *wp_center, float radius) { + struct FloatVect2 pos_diff; + float desired_speed; - // implement nav hybrid circle + VECT2_DIFF(pos_diff, *stateGetPositionEnu_f(), *wp_center); + // direction of rotation + float sign_radius = radius > 0.f ? 1.f : -1.f; + // absolute radius + float abs_radius = fabsf(radius); + + if (abs_radius > 0.1f) { + // store last qdr + float last_qdr = nav_rotorcraft_base.circle.qdr; + // compute qdr + nav_rotorcraft_base.circle.qdr = atan2f(pos_diff.y, pos_diff.x); + // increment circle radians + float trigo_diff = nav_rotorcraft_base.circle.qdr - last_qdr; + NormRadAngle(trigo_diff); + nav_rotorcraft_base.circle.radians += trigo_diff; + // progress angle + float progess_angle = GUIDANCE_INDI_NAV_CIRCLE_DIST / abs_radius; + Bound(progess_angle, M_PI / 16.f, M_PI / 4.f); + float alpha = nav_rotorcraft_base.circle.qdr - sign_radius * progess_angle; + // final target position, should be on the circle, for display + nav.target.x = wp_center->x + cosf(alpha) * abs_radius; + nav.target.y = wp_center->y + sinf(alpha) * abs_radius; + } + else { + // radius is too small, direct to center + VECT2_COPY(nav.target, *wp_center); + } + // compute desired speed + if (force_forward) { + desired_speed = nav_max_speed; + } else { + float radius_diff = fabsf(float_vect2_norm(&pos_diff) - abs_radius); + if (radius_diff > GUIDANCE_INDI_NAV_CIRCLE_DIST) { + // far from circle, speed proportional to diff + desired_speed = radius_diff * gih_params.pos_gain; + } else { + // close to circle, speed function of radius for a feasible turn + // MAX_BANK / 2 gives some margins for the turns + desired_speed = sqrtf(PPRZ_ISA_GRAVITY * abs_radius * tanf(GUIDANCE_H_MAX_BANK / 2.f)); + } + Bound(desired_speed, 0.0f, nav_max_speed); + } + // compute speed vector from target position + struct FloatVect2 speed_unit; + VECT2_DIFF(speed_unit, nav.target, *stateGetPositionEnu_f()); + float_vect2_normalize(&speed_unit); + VECT2_SMUL(nav.speed, speed_unit, desired_speed); nav_rotorcraft_base.circle.center = *wp_center; nav_rotorcraft_base.circle.radius = radius; @@ -184,8 +235,6 @@ static void nav_circle(struct EnuCoor_f *wp_center, float radius) nav.setpoint_mode = NAV_SETPOINT_MODE_SPEED; } -#endif - /** Init and register nav functions * * For hybrid vehicle nav @@ -200,7 +249,7 @@ void nav_rotorcraft_hybrid_init(void) // register nav functions nav_register_goto_wp(nav_hybrid_goto, nav_hybrid_route, nav_hybrid_approaching); - //nav_register_circle(nav_circle); + nav_register_circle(nav_hybrid_circle); }