Paparazzi UAS
v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
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"
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... | |
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.
Definition at line 59 of file wls_alloc.c.
#define WLS_VERBOSE FALSE |
Definition at line 49 of file wls_alloc.c.
|
static |
Wrapper for qr solve.
Possible to use a different solver if needed. Solves a system of the form Ax = b for x.
m | number of rows |
n | number 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().
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
u | The control output vector |
v | The control objective vector |
umin | The minimum u vector |
umax | The maximum u vector |
B | The control effectiveness matrix |
u_guess | Initial value for u |
W_init | Initial working set, if known |
Wv | Weighting on different control objectives |
Wu | Weighting on different controls |
up | Preferred control vector |
gamma_sq | Preference of satisfying control objective over desired control vector (sqare root of gamma) |
imax | Max number of iterations |
n_u | Length of u (the number of actuators) |
n_v | Lenght of v (the number of control objectives) |
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().