Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
wls_alloc.c File Reference

This is an active set algorithm for WLS control allocation. More...

#include "wls_alloc.h"
#include "std.h"
#include <string.h>
#include <math.h>
#include <float.h>
#include "math/qr_solve/qr_solve.h"
#include "math/qr_solve/r8lib_min.h"
+ Include dependency graph for wls_alloc.c:

Go to the source code of this file.

Macros

#define WLS_VERBOSE   FALSE
 
#define WLS_N_C   ((WLS_N_U)+(WLS_N_V))
 

Functions

static void qr_solve_wrapper (int m, int n, float **A, float *b, float *x)
 Wrapper for qr solve. More...
 
int wls_alloc (float *u, float *v, float *umin, float *umax, float **B, float *u_guess, float *W_init, float *Wv, float *Wu, float *up, float gamma_sq, int imax, int n_u, int n_v)
 active set algorithm for control allocation More...
 

Detailed Description

This is an active set algorithm for WLS control allocation.

This algorithm will find the optimal inputs to produce the least error wrt the control objective, taking into account the weighting matrices on the control objective and the control effort.

The algorithm is described in: Prioritized Control Allocation for Quadrotors Subject to Saturation - E.J.J. Smeur, D.C. Höppener, C. de Wagter. In IMAV 2017

written by Anton Naruta && Daniel Hoppener 2016 MAVLab Delft University of Technology

Definition in file wls_alloc.c.

Macro Definition Documentation

◆ WLS_N_C

#define WLS_N_C   ((WLS_N_U)+(WLS_N_V))

Definition at line 59 of file wls_alloc.c.

◆ WLS_VERBOSE

#define WLS_VERBOSE   FALSE

Definition at line 49 of file wls_alloc.c.

Function Documentation

◆ qr_solve_wrapper()

static void qr_solve_wrapper ( int  m,
int  n,
float **  A,
float *  b,
float *  x 
)
static

Wrapper for qr solve.

Possible to use a different solver if needed. Solves a system of the form Ax = b for x.

Parameters
mnumber of rows
nnumber of columns

Definition at line 70 of file wls_alloc.c.

References A, b, simple_quad_sim::m, and qr_solve().

Referenced by wls_alloc().

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

◆ wls_alloc()

int wls_alloc ( float *  u,
float *  v,
float *  umin,
float *  umax,
float **  B,
float *  u_guess,
float *  W_init,
float *  Wv,
float *  Wu,
float *  up,
float  gamma_sq,
int  imax,
int  n_u,
int  n_v 
)

active set algorithm for control allocation

Takes the control objective and max and min inputs from pprz and calculates the inputs that will satisfy most of the control objective, subject to the weighting matrices Wv and Wu

Parameters
uThe control output vector
vThe control objective vector
uminThe minimum u vector
umaxThe maximum u vector
BThe control effectiveness matrix
u_guessInitial value for u
W_initInitial working set, if known
WvWeighting on different control objectives
WuWeighting on different controls
upPreferred control vector
gamma_sqPreference of satisfying control objective over desired control vector (sqare root of gamma)
imaxMax number of iterations
n_uLength of u (the number of actuators)
n_vLenght of v (the number of control objectives)
Returns
Number of iterations which is (imax+1) if it ran out of iterations

Definition at line 108 of file wls_alloc.c.

References A, alpha, B, b, lambda, p, qr_solve_wrapper(), UNUSED, up, WLS_N_C, WLS_N_U, Wu, and Wv.

Referenced by oneloop_andi_run(), and stabilization_indi_rate_run().

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