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 "modules/datalink/telemetry.h"

Include dependency graph for wls_alloc.c:

Go to the source code of this file.

Macros
#define	WLS_VERBOSE   FALSE

Functions
void	send_wls_v (char *name, struct WLS_t *WLS_p, struct transport_tx *trans, struct link_device *dev)
void	send_wls_u (char *name, struct WLS_t *WLS_p, struct transport_tx *trans, struct link_device *dev)
static void	qr_solve_wrapper (int m, int n, float **A, float *b, float *x)
	Wrapper for qr solve. More...
void	wls_alloc (struct WLS_t *WLS_p, float **B, float *u_guess, float *W_init, int imax)
	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_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

m	number of rows
n	number of columns

Definition at line 92 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:

send_wls_u()

void send_wls_u	(	char *	name,
		struct WLS_t *	WLS_p,
		struct transport_tx *	trans,
		struct link_device *	dev
	)

Definition at line 71 of file wls_alloc.c.

References dev, WLS_t::nu, WLS_t::u, WLS_t::u_max, WLS_t::u_min, WLS_t::u_pref, and WLS_t::Wu.

Referenced by send_wls_u_stab().

Here is the caller graph for this function:

send_wls_v()

void send_wls_v	(	char *	name,
		struct WLS_t *	WLS_p,
		struct transport_tx *	trans,
		struct link_device *	dev
	)

Definition at line 61 of file wls_alloc.c.

References dev, WLS_t::gamma_sq, WLS_t::iter, WLS_t::nv, WLS_t::v, and WLS_t::Wv.

Referenced by send_wls_v_stab().

Here is the caller graph for this function:

wls_alloc()

void wls_alloc	(	struct WLS_t *	WLS_p,
		float **	B,
		float *	u_guess,
		float *	W_init,
		int	imax
	)

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

WLS_p	Struct that contains most of the WLS parameters
B	The control effectiveness matrix
u_guess	Initial value for u
W_init	Initial working set, if known
imax	Max number of iterations

Definition at line 119 of file wls_alloc.c.

References A, alpha, B, b, WLS_t::gamma_sq, WLS_t::iter, lambda, WLS_t::nu, WLS_t::nv, p, qr_solve_wrapper(), WLS_t::u, WLS_t::u_max, WLS_t::u_min, WLS_t::u_pref, UNUSED, WLS_t::v, WLS_t::Wu, and WLS_t::Wv.

Referenced by oneloop_andi_run().

Here is the call graph for this function:

Here is the caller graph for this function: