Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
gvf_adapted_step.h File Reference

Dynamic parametric step adaptation for the GVF algorithm. More...

+ This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Functions

float step_adaptation (float ds, float f1d, float f2d, float f3d, float f1dd, float f2dd, float f3dd)
 Compute the adapted parametric step given the wanted geometric distance.
 

Detailed Description

Dynamic parametric step adaptation for the GVF algorithm.

Contains two macro-defined parameters:

  • ADAPTED_STEP_NULL_TOLERANCE: Numbers with absolute values smaller than ADAPTED_STEP_NULL_TOLERANCE will be considered equal to 0. Default to 1e-6
  • ADAPTED_STEP_MAX_ROOTFINDING_STEPS: Maximum number of steps used in the root finding algorithm. Default to 1e6

Definition in file gvf_adapted_step.h.

Function Documentation

◆ step_adaptation()

float step_adaptation ( float  ds,
float  f1d,
float  f2d,
float  f3d,
float  f1dd,
float  f2dd,
float  f3dd 
)

Compute the adapted parametric step given the wanted geometric distance.

Given the 3D parametric curve described by f at p and a wanted geometric distance ds, computes the parametric step dp such that: |ds| = || f(p+dp) - f(p) || and (ds)(dp) > 0

To do so, it uses second order Taylor approximation around p to reduce the problem to finding a root of a degree 4 polynomial. This root is found using Halley's method.

We assume the curve non 2-singular (that is both f' and f'' does not vanish). (If it is the case, then the method falls back to returning dp = ds, and print an error to stderr).

Parameters
dsWanted geometric distance
f1dx value of f'(p)
f2dy value of f'(p)
f3dz value of f'(p)
f1ddx value of f''(p)
f2ddy value of f''(p)
f3ddz value of f''(p)
Returns
float The corresponding parametric step to take

Definition at line 108 of file gvf_adapted_step.c.

References ADAPTED_STEP_MAX_ROOTFINDING_STEPS, ADAPTED_STEP_NULL_TOLERANCE, foo, init, and p4_halley().

Referenced by gvf_parametric_control_2D().

+ Here is the call graph for this function:
+ Here is the caller graph for this function: