Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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>
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>
54static void print_final_values(struct WLS_t* WLS_p, float **B);
55static void print_in_and_outputs(int n_c, int n_free, float **A_free_ptr, float *d, float *p_free);
56#endif
57
58/* Define messages of the module*/
59#if PERIODIC_TELEMETRY
61void send_wls_v(char *name, struct WLS_t *WLS_p, struct transport_tx *trans, struct link_device *dev)
62{
65 strlen(name),name,
66 &WLS_p->gamma_sq,
67 &iter_temp,
68 WLS_p->nv, WLS_p->v,
69 WLS_p->nv, WLS_p->Wv);
70}
71void send_wls_u(char *name, struct WLS_t *WLS_p, struct transport_tx *trans, struct link_device *dev)
72{
74 strlen(name),name,
75 WLS_p->nu, WLS_p->Wu,
76 WLS_p->nu, WLS_p->u_pref,
77 WLS_p->nu, WLS_p->u_min,
78 WLS_p->nu, WLS_p->u_max,
79 WLS_p->nu, WLS_p->u);
80}
81#endif
82
92static void qr_solve_wrapper(int m, int n, float **A, float *b, float *x) {
93 float in[m * n];
94 // convert A to 1d array
95 int k = 0;
96 for (int j = 0; j < n; j++) {
97 for (int i = 0; i < m; i++) {
98 in[k++] = A[i][j];
99 }
100 }
101 // use solver
102 qr_solve(m, n, in, b, x);
103}
104
119void wls_alloc(struct WLS_t* WLS_p, float **B, float *u_guess, float *W_init, int imax) {
120 // allocate variables, use defaults where parameters are set to 0
121 if (!WLS_p->gamma_sq) WLS_p->gamma_sq = 100000;
122 if (!imax) imax = 100;
123
124 int n_c = WLS_p->nu + WLS_p->nv;
125
126 float A[n_c][WLS_p->nu];
127 float A_free[n_c][WLS_p->nu];
128
129 // Create a pointer array to the rows of A_free
130 // such that we can pass it to a function
131 float *A_free_ptr[n_c];
132 for(int i = 0; i < n_c; i++)
133 A_free_ptr[i] = A_free[i];
134
135 float b[n_c];
136 float d[n_c];
137
138 int free_index[WLS_p->nu];
139 int free_index_lookup[WLS_p->nu];
140 int n_free = 0;
141 int free_chk = -1;
142
143 int iter = 0;
144 float p_free[WLS_p->nu];
145 float p[WLS_p->nu];
146 float u_opt[WLS_p->nu];
148 int n_infeasible = 0;
149 float lambda[WLS_p->nu];
150 float W[WLS_p->nu];
151
152 // Initialize u and the working set, if provided from input
153 if (!u_guess) {
154 for (int i = 0; i < WLS_p->nu; i++) {
155 WLS_p->u[i] = (WLS_p->u_max[i] + WLS_p->u_min[i]) * 0.5;
156 }
157 } else {
158 for (int i = 0; i < WLS_p->nu; i++) {
159 WLS_p->u[i] = u_guess[i];
160 }
161 }
162 W_init ? memcpy(W, W_init, WLS_p->nu * sizeof(float))
163 : memset(W, 0, WLS_p->nu * sizeof(float));
164
165 memset(free_index_lookup, -1, WLS_p->nu * sizeof(float));
166
167 // find free indices
168 for (int i = 0; i < WLS_p->nu; i++) {
169 if (W[i] == 0) {
171 free_index[n_free++] = i;
172 }
173 }
174
175 // fill up A, A_free, b and d
176 for (int i = 0; i < WLS_p->nv; i++) {
177 b[i] = WLS_p->gamma_sq * WLS_p->Wv[i] * WLS_p->v[i];
178 d[i] = b[i];
179 for (int j = 0; j < WLS_p->nu; j++) {
180 // If Wv is a NULL pointer, use Wv = identity
181 A[i][j] = WLS_p->gamma_sq * WLS_p->Wv[i] * B[i][j];
182 d[i] -= A[i][j] * WLS_p->u[j];
183 }
184 }
185 for (int i = WLS_p->nv; i < n_c; i++) {
186 memset(A[i], 0, WLS_p->nu * sizeof(float));
187 A[i][i - WLS_p->nv] = WLS_p->Wu[i - WLS_p->nv];
188 b[i] = WLS_p->Wu[i - WLS_p->nv] * WLS_p->u_pref[i - WLS_p->nv];
189 d[i] = b[i] - A[i][i - WLS_p->nv] * WLS_p->u[i - WLS_p->nv];
190 }
191
192 // -------------- Start loop ------------
193 while (iter++ < imax) {
194 // clear p, copy u to u_opt
195 memset(p, 0, WLS_p->nu * sizeof(float));
196 memcpy(u_opt, WLS_p->u, WLS_p->nu * sizeof(float));
197
198 // Construct a matrix with the free columns of A
199 if (free_chk != n_free) {
200 for (int i = 0; i < n_c; i++) {
201 for (int j = 0; j < n_free; j++) {
202 A_free[i][j] = A[i][free_index[j]];
203 }
204 }
206 }
207
208
209 // Count the infeasible free actuators
210 n_infeasible = 0;
211
212 if (n_free > 0) {
213 // Still free variables left, calculate corresponding solution
214
215 // use a solver to find the solution to A_free*p_free = d
217
218 //print results current step
219#if WLS_VERBOSE
221#endif
222
223 // Set the nonzero values of p and add to u_opt
224 for (int i = 0; i < n_free; i++) {
225 p[free_index[i]] = p_free[i];
226 u_opt[free_index[i]] += p_free[i];
227
228 // check limits
229 if ((u_opt[free_index[i]] > WLS_p->u_max[free_index[i]] || u_opt[free_index[i]] < WLS_p->u_min[free_index[i]])) {
231 }
232 }
233 }
234
235 // Check feasibility of the solution
236 if (n_infeasible == 0) {
237 // all variables are within limits
238 memcpy(WLS_p->u, u_opt, WLS_p->nu * sizeof(float));
239 memset(lambda, 0, WLS_p->nu * sizeof(float));
240
241 // d = d + A_free*p_free; lambda = A*d;
242 for (int i = 0; i < n_c; i++) {
243 for (int k = 0; k < n_free; k++) {
244 d[i] -= A_free[i][k] * p_free[k];
245 }
246 for (int k = 0; k < WLS_p->nu; k++) {
247 lambda[k] += A[i][k] * d[i];
248 }
249 }
250 bool break_flag = true;
251
252 // lambda = lambda x W;
253 for (int i = 0; i < WLS_p->nu; i++) {
254 lambda[i] *= W[i];
255 // if any lambdas are negative, keep looking for solution
256 if (lambda[i] < -FLT_EPSILON) {
257 break_flag = false;
258 W[i] = 0;
259 // add a free index
260 if (free_index_lookup[i] < 0) {
262 free_index[n_free++] = i;
263 }
264 }
265 }
266 if (break_flag) {
267
268#if WLS_VERBOSE
270#endif
271
272 // if solution is found, return number of iterations
273 WLS_p->iter = iter;
274 }
275 } else {
276 // scaling back actuator command (0-1)
277 float alpha = 1.0;
278 float alpha_tmp;
279 int id_alpha = free_index[0];
280
281 // find the lowest distance from the limit among the free variables
282 for (int i = 0; i < n_free; i++) {
283 int id = free_index[i];
284
285 alpha_tmp = (p[id] < 0) ? (WLS_p->u_min[id] - WLS_p->u[id]) / p[id]
286 : (WLS_p->u_max[id] - WLS_p->u[id]) / p[id];
287
288 if (isnan(alpha_tmp) || alpha_tmp < 0.f) {
289 alpha_tmp = 1.0f;
290 }
291 if (alpha_tmp < alpha) {
293 id_alpha = id;
294 }
295 }
296
297 // update input u = u + alpha*p
298 for (int i = 0; i < WLS_p->nu; i++) {
299 WLS_p->u[i] += alpha * p[i];
300 Bound(WLS_p->u[i], WLS_p->u_min[i], WLS_p->u_max[i]);
301 }
302 // update d = d-alpha*A*p_free
303 for (int i = 0; i < n_c; i++) {
304 for (int k = 0; k < n_free; k++) {
305 d[i] -= A_free[i][k] * alpha * p_free[k];
306 }
307 }
308 // get rid of a free index
309 W[id_alpha] = (p[id_alpha] > 0) ? 1.0 : -1.0;
310
315 }
316 }
317 WLS_p->iter = iter;
318}
319
320#if WLS_VERBOSE
321static void print_in_and_outputs(int n_c, int n_free, float **A_free_ptr, float *d, float *p_free) {
322 printf("n_c = %d n_free = %d\n", n_c, n_free);
323
324 printf("A_free =\n");
325 for (int i = 0; i < n_c; i++) {
326 for (int j = 0; j < n_free; j++) {
327 printf("%f ", A_free_ptr[i][j]);
328 }
329 printf("\n");
330 }
331
332 printf("d = ");
333 for (int j = 0; j < n_c; j++) {
334 printf("%f ", d[j]);
335 }
336
337 printf("\noutput = ");
338 for (int j = 0; j < n_free; j++) {
339 printf("%f ", p_free[j]);
340 }
341 printf("\n\n");
342}
343
344static void print_final_values(struct WLS_t* WLS_p, float **B) {
345 printf("n_u = %d n_v = %d\n", WLS_p->nu, WLS_p->nv);
346
347 printf("B =\n");
348 for (int i = 0; i < WLS_p->nv; i++) {
349 for (int j = 0; j < WLS_p->nu; j++) {
350 printf("%f ", B[i][j]);
351 }
352 printf("\n");
353 }
354
355 printf("v = ");
356 for (int j = 0; j < WLS_p->nv; j++) {
357 printf("%f ", WLS_p->v[j]);
358 }
359
360 printf("\nu = ");
361 for (int j = 0; j < WLS_p->nu; j++) {
362 printf("%f ", u[j]);
363 }
364 printf("\n");
365
366 printf("\numin = ");
367 for (int j = 0; j < WLS_p->nu; j++) {
368 printf("%f ", WLS_p->u_min[j]);
369 }
370 printf("\n");
371
372 printf("\numax = ");
373 for (int j = 0; j < WLS_p->nu; j++) {
374 printf("%f ", WLS_p->u_max[j]);
375 }
376 printf("\n\n");
377
378}
379#endif
#define B
#define UNUSED(x)
#define A
static float p[2][2]
uint16_t foo
Definition main_demo5.c:58
float lambda
void qr_solve(int m, int n, float a[], float b[], float x[])
Definition qr_solve.c:1548
static const struct usb_device_descriptor dev
Definition usb_ser_hw.c:74
Periodic telemetry system header (includes downlink utility and generated code).
float alpha
Definition textons.c:133
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
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:92
void send_wls_v(char *name, struct WLS_t *WLS_p, struct transport_tx *trans, struct link_device *dev)
Definition wls_alloc.c:61
void wls_alloc(struct WLS_t *WLS_p, float **B, float *u_guess, float *W_init, int imax)
active set algorithm for control allocation
Definition wls_alloc.c:119
void send_wls_u(char *name, struct WLS_t *WLS_p, struct transport_tx *trans, struct link_device *dev)
Definition wls_alloc.c:71