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

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

#include <math.h>
#include "std.h"
#include "gvf_adapted_step.h"
+ Include dependency graph for gvf_adapted_step.c:

Go to the source code of this file.

Macros

#define ADAPTED_STEP_NULL_TOLERANCE   1e-6
 
#define ADAPTED_STEP_MAX_ROOTFINDING_STEPS   1e6
 

Functions

static float p4_eval (float x, float a4, float a3, float a2, float a1, float a0)
 Horner's method for fast degree 4 polynomial evaluation.
 
static float p4_halley (float a4, float a3, float a2, float a1, float a0, float tol, float init, int max_steps)
 Implementation of Halley's method for degree 4 polynomial root finding.
 
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.

Definition in file gvf_adapted_step.c.

Macro Definition Documentation

◆ ADAPTED_STEP_MAX_ROOTFINDING_STEPS

#define ADAPTED_STEP_MAX_ROOTFINDING_STEPS   1e6

Definition at line 40 of file gvf_adapted_step.c.

◆ ADAPTED_STEP_NULL_TOLERANCE

#define ADAPTED_STEP_NULL_TOLERANCE   1e-6

Definition at line 35 of file gvf_adapted_step.c.

Function Documentation

◆ p4_eval()

static float p4_eval ( float  x,
float  a4,
float  a3,
float  a2,
float  a1,
float  a0 
)
static

Horner's method for fast degree 4 polynomial evaluation.

Cf https://en.wikipedia.org/wiki/Horner's_method

Parameters
x: Evaluation point for the polynomial
a4: Degree 4 coefficient of the polynomial
a3: Degree 3 coefficient of the polynomial
a2: Degree 2 coefficient of the polynomial
a1: Degree 1 coefficient of the polynomial
a0: Degree 0 coefficient of the polynomial
Returns
float : Value of the polynomial evaluated at x

Definition at line 56 of file gvf_adapted_step.c.

References foo.

Referenced by p4_halley().

+ Here is the caller graph for this function:

◆ p4_halley()

static float p4_halley ( float  a4,
float  a3,
float  a2,
float  a1,
float  a0,
float  tol,
float  init,
int  max_steps 
)
static

Implementation of Halley's method for degree 4 polynomial root finding.

Cf https://en.wikipedia.org/wiki/Halley%27s_method

Parameters
a4: Degree 4 coefficient of the polynomial
a3: Degree 3 coefficient of the polynomial
a2: Degree 2 coefficient of the polynomial
a1: Degree 1 coefficient of the polynomial
a0: Degree 0 coefficient of the polynomial
tol: Tolerance; the root is found if the absolute value of the polynomial evaluation is below the tolerance
init: Initial guess for the root
max_steps: Maximal number of iterations allowed
Returns
float : The guessed root (or whatever value is left after max_steps iteration)

Definition at line 76 of file gvf_adapted_step.c.

References foo, init, p4_eval(), and step.

Referenced by step_adaptation().

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

◆ 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: