Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
wls_alloc.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) Anton Naruta && Daniel Hoppener
3  * MAVLab Delft University of Technology
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
38 #include "wls_alloc.h"
39 #include "std.h"
40 
41 #include <string.h>
42 #include <math.h>
43 #include <float.h>
44 #include "math/qr_solve/qr_solve.h"
46 
47 // provide loop feedback
48 #ifndef WLS_VERBOSE
49 #define WLS_VERBOSE FALSE
50 #endif
51 
52 #if WLS_VERBOSE
53 #include <stdio.h>
54 static void print_final_values(int n_u, int n_v, float* u, float** B, float* v, float* umin, float* umax);
55 static void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, float* p_free);
56 #endif
57 
58 
59 #define WLS_N_C ((WLS_N_U)+(WLS_N_V))
60 
70 static void qr_solve_wrapper(int m, int n, float** A, float* b, float* x) {
71  float in[m * n];
72  // convert A to 1d array
73  int k = 0;
74  for (int j = 0; j < n; j++) {
75  for (int i = 0; i < m; i++) {
76  in[k++] = A[i][j];
77  }
78  }
79  // use solver
80  qr_solve(m, n, in, b, x);
81 }
82 
108 int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
109  float* u_guess, float* W_init, float* Wv, float* Wu, float* up,
110  float gamma_sq, int imax, int n_u, int n_v) {
111  // allocate variables, use defaults where parameters are set to 0
112  if(!gamma_sq) gamma_sq = 100000;
113  if(!imax) imax = 100;
114 
115  int n_c = n_u + n_v;
116 
117  float A[WLS_N_C][WLS_N_U];
118  float A_free[WLS_N_C][WLS_N_U];
119 
120  // Create a pointer array to the rows of A_free
121  // such that we can pass it to a function
122  float * A_free_ptr[WLS_N_C];
123  for(int i = 0; i < n_c; i++)
124  A_free_ptr[i] = A_free[i];
125 
126  float b[WLS_N_C];
127  float d[WLS_N_C];
128 
129  int free_index[WLS_N_U];
130  int free_index_lookup[WLS_N_U];
131  int n_free = 0;
132  int free_chk = -1;
133 
134  int iter = 0;
135  float p_free[WLS_N_U];
136  float p[WLS_N_U];
137  float u_opt[WLS_N_U];
138  int infeasible_index[WLS_N_U] UNUSED;
139  int n_infeasible = 0;
140  float lambda[WLS_N_U];
141  float W[WLS_N_U];
142 
143  // Initialize u and the working set, if provided from input
144  if (!u_guess) {
145  for (int i = 0; i < n_u; i++) {
146  u[i] = (umax[i] + umin[i]) * 0.5;
147  }
148  } else {
149  for (int i = 0; i < n_u; i++) {
150  u[i] = u_guess[i];
151  }
152  }
153  W_init ? memcpy(W, W_init, n_u * sizeof(float))
154  : memset(W, 0, n_u * sizeof(float));
155 
156  memset(free_index_lookup, -1, n_u * sizeof(float));
157 
158  // find free indices
159  for (int i = 0; i < n_u; i++) {
160  if (W[i] == 0) {
161  free_index_lookup[i] = n_free;
162  free_index[n_free++] = i;
163  }
164  }
165 
166  // fill up A, A_free, b and d
167  for (int i = 0; i < n_v; i++) {
168  // If Wv is a NULL pointer, use Wv = identity
169  b[i] = Wv ? gamma_sq * Wv[i] * v[i] : gamma_sq * v[i];
170  d[i] = b[i];
171  for (int j = 0; j < n_u; j++) {
172  // If Wv is a NULL pointer, use Wv = identity
173  A[i][j] = Wv ? gamma_sq * Wv[i] * B[i][j] : gamma_sq * B[i][j];
174  d[i] -= A[i][j] * u[j];
175  }
176  }
177  for (int i = n_v; i < n_c; i++) {
178  memset(A[i], 0, n_u * sizeof(float));
179  A[i][i - n_v] = Wu ? Wu[i - n_v] : 1.0;
180  b[i] = up ? (Wu ? Wu[i-n_v] * up[i-n_v] : up[i-n_v]) : 0;
181  d[i] = b[i] - A[i][i - n_v] * u[i - n_v];
182  }
183 
184  // -------------- Start loop ------------
185  while (iter++ < imax) {
186  // clear p, copy u to u_opt
187  memset(p, 0, n_u * sizeof(float));
188  memcpy(u_opt, u, n_u * sizeof(float));
189 
190  // Construct a matrix with the free columns of A
191  if (free_chk != n_free) {
192  for (int i = 0; i < n_c; i++) {
193  for (int j = 0; j < n_free; j++) {
194  A_free[i][j] = A[i][free_index[j]];
195  }
196  }
197  free_chk = n_free;
198  }
199 
200 
201  // Count the infeasible free actuators
202  n_infeasible = 0;
203 
204  if (n_free > 0) {
205  // Still free variables left, calculate corresponding solution
206 
207  // use a solver to find the solution to A_free*p_free = d
208  qr_solve_wrapper(n_c, n_free, A_free_ptr, d, p_free);
209 
210  //print results current step
211 #if WLS_VERBOSE
212  print_in_and_outputs(n_c, n_free, A_free_ptr, d, p_free);
213 #endif
214 
215  // Set the nonzero values of p and add to u_opt
216  for (int i = 0; i < n_free; i++) {
217  p[free_index[i]] = p_free[i];
218  u_opt[free_index[i]] += p_free[i];
219 
220  // check limits
221  if ( (u_opt[free_index[i]] > umax[free_index[i]] || u_opt[free_index[i]] < umin[free_index[i]])) {
222  infeasible_index[n_infeasible++] = free_index[i];
223  }
224  }
225  }
226 
227  // Check feasibility of the solution
228  if (n_infeasible == 0) {
229  // all variables are within limits
230  memcpy(u, u_opt, n_u * sizeof(float));
231  memset(lambda, 0, n_u * sizeof(float));
232 
233  // d = d + A_free*p_free; lambda = A*d;
234  for (int i = 0; i < n_c; i++) {
235  for (int k = 0; k < n_free; k++) {
236  d[i] -= A_free[i][k] * p_free[k];
237  }
238  for (int k = 0; k < n_u; k++) {
239  lambda[k] += A[i][k] * d[i];
240  }
241  }
242  bool break_flag = true;
243 
244  // lambda = lambda x W;
245  for (int i = 0; i < n_u; i++) {
246  lambda[i] *= W[i];
247  // if any lambdas are negative, keep looking for solution
248  if (lambda[i] < -FLT_EPSILON) {
249  break_flag = false;
250  W[i] = 0;
251  // add a free index
252  if (free_index_lookup[i] < 0) {
253  free_index_lookup[i] = n_free;
254  free_index[n_free++] = i;
255  }
256  }
257  }
258  if (break_flag) {
259 
260 #if WLS_VERBOSE
261  print_final_values(n_u, n_v, u, B, v, umin, umax);
262 #endif
263 
264  // if solution is found, return number of iterations
265  return iter;
266  }
267  } else {
268  // scaling back actuator command (0-1)
269  float alpha = 1.0;
270  float alpha_tmp;
271  int id_alpha = free_index[0];
272 
273  // find the lowest distance from the limit among the free variables
274  for (int i = 0; i < n_free; i++) {
275  int id = free_index[i];
276 
277  alpha_tmp = (p[id] < 0) ? (umin[id] - u[id]) / p[id]
278  : (umax[id] - u[id]) / p[id];
279 
280  if (isnan(alpha_tmp) || alpha_tmp < 0.f) {
281  alpha_tmp = 1.0f;
282  }
283  if (alpha_tmp < alpha) {
284  alpha = alpha_tmp;
285  id_alpha = id;
286  }
287  }
288 
289  // update input u = u + alpha*p
290  for (int i = 0; i < n_u; i++) {
291  u[i] += alpha * p[i];
292  Bound(u[i],umin[i],umax[i]);
293  }
294  // update d = d-alpha*A*p_free
295  for (int i = 0; i < n_c; i++) {
296  for (int k = 0; k < n_free; k++) {
297  d[i] -= A_free[i][k] * alpha * p_free[k];
298  }
299  }
300  // get rid of a free index
301  W[id_alpha] = (p[id_alpha] > 0) ? 1.0 : -1.0;
302 
303  free_index[free_index_lookup[id_alpha]] = free_index[--n_free];
304  free_index_lookup[free_index[free_index_lookup[id_alpha]]] =
305  free_index_lookup[id_alpha];
306  free_index_lookup[id_alpha] = -1;
307  }
308  }
309  return iter;
310 }
311 
312 #if WLS_VERBOSE
313 static void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, float* p_free) {
314 
315  printf("n_c = %d n_free = %d\n", n_c, n_free);
316 
317  printf("A_free =\n");
318  for(int i = 0; i < n_c; i++) {
319  for (int j = 0; j < n_free; j++) {
320  printf("%f ", A_free_ptr[i][j]);
321  }
322  printf("\n");
323  }
324 
325  printf("d = ");
326  for (int j = 0; j < n_c; j++) {
327  printf("%f ", d[j]);
328  }
329 
330  printf("\noutput = ");
331  for (int j = 0; j < n_free; j++) {
332  printf("%f ", p_free[j]);
333  }
334  printf("\n\n");
335 }
336 
337 static void print_final_values(int n_u, int n_v, float* u, float** B, float* v, float* umin, float* umax) {
338  printf("n_u = %d n_v = %d\n", n_u, n_v);
339 
340  printf("B =\n");
341  for(int i = 0; i < n_v; i++) {
342  for (int j = 0; j < n_u; j++) {
343  printf("%f ", B[i][j]);
344  }
345  printf("\n");
346  }
347 
348  printf("v = ");
349  for (int j = 0; j < n_v; j++) {
350  printf("%f ", v[j]);
351  }
352 
353  printf("\nu = ");
354  for (int j = 0; j < n_u; j++) {
355  printf("%f ", u[j]);
356  }
357  printf("\n");
358 
359  printf("\numin = ");
360  for (int j = 0; j < n_u; j++) {
361  printf("%f ", umin[j]);
362  }
363  printf("\n");
364 
365  printf("\numax = ");
366  for (int j = 0; j < n_u; j++) {
367  printf("%f ", umax[j]);
368  }
369  printf("\n\n");
370 
371 }
372 #endif
#define B
uint8_t last_wp UNUSED
#define A
static float p[2][2]
uint16_t up[LIGHT_NB]
Definition: light_solar.c:48
static float Wv[ANDI_OUTPUTS]
Definition: oneloop_andi.c:200
static float Wu[ANDI_NUM_ACT_TOT]
Definition: oneloop_andi.c:207
float lambda
void qr_solve(int m, int n, float a[], float b[], float x[])
Definition: qr_solve.c:1548
float alpha
Definition: textons.c:133
float b
Definition: wedgebug.c:202
static void qr_solve_wrapper(int m, int n, float **A, float *b, float *x)
Wrapper for qr solve.
Definition: wls_alloc.c:70
#define WLS_N_C
Definition: wls_alloc.c:59
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
Definition: wls_alloc.c:108
#define WLS_N_U
active set algorithm for control allocation
Definition: wls_alloc.h:58