mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
Merge pull request #2077 from paparazzi/revert-2075-allocation_merge
Revert "WLS control allocation"
This commit is contained in:
Ewoud Smeur
GitHub
@@ -100,12 +100,30 @@
|
||||
<!-- setpoint limits for attitude stabilization rc flight -->
|
||||
<define name="SP_MAX_PHI" value="45" unit="deg"/>
|
||||
<define name="SP_MAX_THETA" value="45" unit="deg"/>
|
||||
<define name="SP_MAX_R" value="400" unit="deg/s"/>
|
||||
<define name="SP_MAX_R" value="120" unit="deg/s"/>
|
||||
<define name="DEADBAND_A" value="0"/>
|
||||
<define name="DEADBAND_E" value="0"/>
|
||||
<define name="DEADBAND_R" value="50"/>
|
||||
</section>
|
||||
|
||||
<section name="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
|
||||
<!-- attitude reference generation model -->
|
||||
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
|
||||
<define name="REF_ZETA_P" value="0.9"/>
|
||||
<define name="REF_MAX_P" value="600." unit="deg/s"/>
|
||||
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
|
||||
|
||||
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
|
||||
<define name="REF_ZETA_Q" value="0.9"/>
|
||||
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
|
||||
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
|
||||
|
||||
<define name="REF_OMEGA_R" value="450" unit="deg/s"/>
|
||||
<define name="REF_ZETA_R" value="0.9"/>
|
||||
<define name="REF_MAX_R" value="600." unit="deg/s"/>
|
||||
<define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
|
||||
<!-- control effectiveness -->
|
||||
<define name="G1_ROLL" value="{20 , -20, -20 , 20 }"/>
|
||||
@@ -123,6 +141,9 @@
|
||||
<define name="REF_RATE_Q" value="28.0"/>
|
||||
<define name="REF_RATE_R" value="28.0"/>
|
||||
|
||||
<!--Maxium yaw rate, to avoid instability-->
|
||||
<define name="MAX_R" value="120.0" unit="deg/s"/>
|
||||
|
||||
<define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
|
||||
<define name="FILT_CUTOFF" value="5.0"/>
|
||||
|
||||
|
||||
@@ -11,12 +11,25 @@
|
||||
<define name="SP_MAX_R" value="90." description="max setpoint for yaw rate" unit="deg/s"/>
|
||||
<define name="DEADBAND_R" value="250" description="deadband on yaw rate input"/>
|
||||
</section>
|
||||
<section name="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
|
||||
<define name="REF_OMEGA_P" value="400" description="reference generator omega param on roll rate" unit="deg/s"/>
|
||||
<define name="REF_ZETA_P" value="0.9" description="reference generator zeta param on roll rate"/>
|
||||
<define name="REF_MAX_P" value="300." description="reference generator max roll rate" unit="deg/s"/>
|
||||
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)" description="reference generator max roll acceleration"/>
|
||||
<define name="REF_OMEGA_Q" value="400" description="reference generator omega param on pitch rate" unit="deg/s"/>
|
||||
<define name="REF_ZETA_Q" value="0.9" description="reference generator zeta param on pitch rate"/>
|
||||
<define name="REF_MAX_Q" value="300." description="reference generator max pitch rate" unit="deg/s"/>
|
||||
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)" description="reference generator max pitch acceleration"/>
|
||||
<define name="REF_OMEGA_R" value="250" description="reference generator omega param on yaw rate" unit="deg/s"/>
|
||||
<define name="REF_ZETA_R" value="0.9" description="reference generator zeta param on yaw rate"/>
|
||||
<define name="REF_MAX_R" value="180." description="reference generator max yaw rate" unit="deg/s"/>
|
||||
<define name="REF_MAX_RDOT" value="RadOfDeg(1800.)" description="reference generator max yaw acceleration"/>
|
||||
</section>
|
||||
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
|
||||
<define name="G1_ROLL" value="{20 , -20, -20 , 20 }" description="control effectiveness of every actuator on the roll axis"/>
|
||||
<define name="G1_PITCH" value="{14 , 14, -14 , -14 }" description="control effectiveness of every actuator on the pitch axis"/>
|
||||
<define name="G1_YAW" value="{-1, 1, -1, 1}" description="control effectiveness of every actuator on the yaw axis"/>
|
||||
<define name="G1_THRUST" value="{-.4, -.4, -.4, -.4}" description="control effectiveness of every actuator on the thrust axis"/>
|
||||
<define name="G2" value="{-60.0, 60.0, -60.0, 60.0}" description="control effectiveness of every actuator derivative on the yaw axis (important for propellers with strong torque changes)"/>
|
||||
<define name="G1_P" value="0.0639" description="control effectiveness G1 gain on roll rate"/>
|
||||
<define name="G1_Q" value="0.0361" description="control effectiveness G1 gain on pitch rate"/>
|
||||
<define name="G1_R" value="0.0022" description="control effectiveness G1 gain on yaw rate"/>
|
||||
<define name="G2_R" value="0.1450" description="control effectiveness G2 gain on yaw rate"/>
|
||||
<define name="REF_ERR_P" value="600.0" description="reference acceleration"/>
|
||||
<define name="REF_ERR_Q" value="600.0" description="reference acceleration"/>
|
||||
<define name="REF_ERR_R" value="600.0" description="reference acceleration"/>
|
||||
@@ -26,10 +39,9 @@
|
||||
<define name="MAX_R" value="120.0" description="max yaw rate" unit="deg/s"/>
|
||||
<define name="FILT_CUTOFF" value="8.0" description="second order cutoff parameter"/>
|
||||
<define name="ESTIMATION_FILT_CUTOFF" value="8.0" description="second order cutoff parameter"/>
|
||||
<define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}" description="actuator dynamics"/>
|
||||
<define name="ACT_IS_SERVO" value="{0,0,0,0}" description="1 for every actuator that is a servo"/>
|
||||
<define name="ACT_RATE_LIMIT" value="{9600,9600,9600,9600}" description="rate limit in PPRZ units per timestep (depends on control frequency)"/>
|
||||
<define name="ACT_PREF" value="{0.0, 0.0, 0.0, 0.0}" description="preferred (low energy) actuator value. Important when the system is over-determined!"/>
|
||||
<define name="ACT_DYN_P" value="0.1" description="first order actuator dynamics on roll rate"/>
|
||||
<define name="ACT_DYN_Q" value="0.1" description="first order actuator dynamics on pitch rate"/>
|
||||
<define name="ACT_DYN_R" value="0.1" description="first order actuator dynamics on yaw rate"/>
|
||||
<define name="USE_ADAPTIVE" value="FALSE|TRUE" description="enable adaptive gains"/>
|
||||
<define name="ADAPTIVE_MU" value="0.0001" description="adaptation parameter"/>
|
||||
</section>
|
||||
@@ -57,9 +69,6 @@
|
||||
<file name="stabilization_attitude_quat_indi.c" dir="$(SRC_FIRMWARE)/stabilization"/>
|
||||
<file name="stabilization_attitude_quat_transformations.c" dir="$(SRC_FIRMWARE)/stabilization"/>
|
||||
<file name="stabilization_attitude_rc_setpoint.c" dir="$(SRC_FIRMWARE)/stabilization"/>
|
||||
<file name="wls_alloc.c" dir="$(SRC_FIRMWARE)/stabilization/wls"/>
|
||||
<file name="qr_solve.c" dir="$(SRC_FIRMWARE)/stabilization/wls"/>
|
||||
<file name="r8lib_min.c" dir="$(SRC_FIRMWARE)/stabilization/wls"/>
|
||||
<define name="STABILIZATION_ATTITUDE_TYPE_INT"/>
|
||||
<define name="STABILIZATION_ATTITUDE_TYPE_H" value="stabilization/stabilization_attitude_quat_indi.h" type="string"/>
|
||||
<define name="STABILIZATION_ATTITUDE_INDI_FULL" value="true"/>
|
||||
|
||||
@@ -42,15 +42,6 @@
|
||||
#include "subsystems/actuators.h"
|
||||
#include "subsystems/abi.h"
|
||||
#include "filters/low_pass_filter.h"
|
||||
#include "wls/wls_alloc.h"
|
||||
#include <stdio.h>
|
||||
|
||||
float du_min[4];
|
||||
float du_max[4];
|
||||
float du_pref[4];
|
||||
float indi_v[4];
|
||||
float* Bwls[4];
|
||||
int num_iter = 0;
|
||||
|
||||
static void lms_estimation(void);
|
||||
static void get_actuator_state(void);
|
||||
@@ -94,14 +85,6 @@ bool act_is_servo[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_SERVO;
|
||||
bool act_is_servo[INDI_NUM_ACT] = {0};
|
||||
#endif
|
||||
|
||||
#ifdef STABILIZATION_INDI_ACT_PREF
|
||||
// Preferred (neutral, least energy) actuator value
|
||||
float act_pref[4] = STABILIZATION_INDI_ACT_PREF;
|
||||
#else
|
||||
// Assume 0 is neutral
|
||||
float act_pref[4] = {0.0};
|
||||
#endif
|
||||
|
||||
float act_dyn[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN;
|
||||
|
||||
/** Maximum rate you can request in RC rate mode (rad/s)*/
|
||||
@@ -210,12 +193,6 @@ void stabilization_indi_init(void)
|
||||
//Calculate G1G2_PSEUDO_INVERSE
|
||||
calc_g1g2_pseudo_inv();
|
||||
|
||||
// Initialize the array of pointers to the rows of g1g2
|
||||
uint8_t i;
|
||||
for(i=0; i<INDI_OUTPUTS; i++) {
|
||||
Bwls[i] = g1g2[i];
|
||||
}
|
||||
|
||||
// Initialize the estimator matrices
|
||||
float_vect_copy(g1_est[0], g1[0], INDI_OUTPUTS*INDI_NUM_ACT);
|
||||
float_vect_copy(g2_est, g2, INDI_NUM_ACT);
|
||||
@@ -319,7 +296,7 @@ void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
|
||||
|
||||
quat_from_earth_cmd_i(&stab_att_sp_quat, cmd, heading);
|
||||
}
|
||||
#include "subsystems/radio_control.h"
|
||||
|
||||
/**
|
||||
* @param att_err attitude error
|
||||
* @param rate_control boolean that states if we are in rate control or attitude control
|
||||
@@ -363,52 +340,45 @@ static void stabilization_indi_calc_cmd(struct Int32Quat *att_err, bool rate_con
|
||||
//G2 is scaled by INDI_G_SCALING to make it readable
|
||||
g2_times_du = g2_times_du/INDI_G_SCALING;
|
||||
|
||||
float v_thrust = 0.0;
|
||||
if(indi_thrust_increment_set){
|
||||
v_thrust = indi_thrust_increment;
|
||||
|
||||
//update thrust command such that the current is correctly estimated
|
||||
stabilization_cmd[COMMAND_THRUST] = (actuator_state[0] + actuator_state[1] + actuator_state[2] + actuator_state[3])/4.0;
|
||||
} else {
|
||||
// incremental thrust
|
||||
for(i=0; i<INDI_NUM_ACT; i++) {
|
||||
v_thrust +=
|
||||
(stabilization_cmd[COMMAND_THRUST] - actuator_state_filt_vect[i])*Bwls[3][i];
|
||||
}
|
||||
}
|
||||
|
||||
// Calculate the min and max increments
|
||||
for(i=0; i<INDI_NUM_ACT; i++) {
|
||||
du_min[i] = -MAX_PPRZ*act_is_servo[i] - actuator_state_filt_vect[i];
|
||||
du_max[i] = MAX_PPRZ - actuator_state_filt_vect[i];
|
||||
du_pref[i] = act_pref[i] - actuator_state_filt_vect[i];
|
||||
}
|
||||
|
||||
//State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST}
|
||||
static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100};
|
||||
|
||||
// The control objective in array format
|
||||
indi_v[0] = (angular_accel_ref.p - angular_acceleration[0]);
|
||||
indi_v[1] = (angular_accel_ref.q - angular_acceleration[1]);
|
||||
indi_v[2] = (angular_accel_ref.r - angular_acceleration[2] + g2_times_du);
|
||||
indi_v[3] = v_thrust;
|
||||
|
||||
#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
|
||||
// Calculate the increment for each actuator
|
||||
for(i=0; i<INDI_NUM_ACT; i++) {
|
||||
indi_du[i] = (g1g2_pseudo_inv[i][0] * indi_v[0])
|
||||
+ (g1g2_pseudo_inv[i][1] * indi_v[1])
|
||||
+ (g1g2_pseudo_inv[i][2] * indi_v[2])
|
||||
+ (g1g2_pseudo_inv[i][3] * indi_v[3]);
|
||||
indi_du[i] = (g1g2_pseudo_inv[i][0] * (angular_accel_ref.p - angular_acceleration[0]))
|
||||
+ (g1g2_pseudo_inv[i][1] * (angular_accel_ref.q - angular_acceleration[1]))
|
||||
+ (g1g2_pseudo_inv[i][2] * (angular_accel_ref.r - angular_acceleration[2] + g2_times_du));
|
||||
}
|
||||
#else
|
||||
// WLS Control Allocator
|
||||
num_iter =
|
||||
wls_alloc(indi_du,indi_v,du_min,du_max,Bwls,INDI_NUM_ACT,INDI_OUTPUTS,0,0,Wv,0,du_min,10000,10);
|
||||
#endif
|
||||
|
||||
// Add the increments to the actuators
|
||||
float_vect_sum(indi_u, actuator_state_filt_vect, indi_du, INDI_NUM_ACT);
|
||||
if(indi_thrust_increment_set){
|
||||
// The required body-z acceleration is calculated by the outer loop INDI controller
|
||||
for(i=0; i<INDI_NUM_ACT; i++) {
|
||||
indi_du[i] = indi_du[i] + g1g2_pseudo_inv[i][3]*indi_thrust_increment;
|
||||
}
|
||||
|
||||
// Add the increments to the actuators
|
||||
float_vect_sum(indi_u, actuator_state_filt_vect, indi_du, INDI_NUM_ACT);
|
||||
} else
|
||||
{
|
||||
// Add the increments to the actuators without the thrust
|
||||
float_vect_sum(indi_u, actuator_state_filt_vect, indi_du, INDI_NUM_ACT);
|
||||
|
||||
// Calculate the average of the actuators as a measure for the thrust
|
||||
float avg_u_in = 0;
|
||||
for(i=0; i<INDI_NUM_ACT; i++) {
|
||||
avg_u_in += indi_u[i];
|
||||
}
|
||||
avg_u_in /= INDI_NUM_ACT;
|
||||
|
||||
// Make sure the thrust is bounded
|
||||
Bound(stabilization_cmd[COMMAND_THRUST],0, MAX_PPRZ);
|
||||
|
||||
//avoid dividing by zero
|
||||
if(avg_u_in < 1.0) {
|
||||
avg_u_in = 1.0;
|
||||
}
|
||||
|
||||
// Rescale the command to the actuators to get the desired thrust
|
||||
float indi_cmd_scaling = stabilization_cmd[COMMAND_THRUST] / avg_u_in;
|
||||
float_vect_smul(indi_u, indi_u, indi_cmd_scaling, INDI_NUM_ACT);
|
||||
}
|
||||
|
||||
// Bound the inputs to the actuators
|
||||
for(i=0; i<INDI_NUM_ACT; i++) {
|
||||
@@ -419,11 +389,6 @@ static void stabilization_indi_calc_cmd(struct Int32Quat *att_err, bool rate_con
|
||||
}
|
||||
}
|
||||
|
||||
if(radio_control.values[RADIO_THROTTLE] < 300) {
|
||||
float_vect_zero(indi_u, INDI_NUM_ACT);
|
||||
float_vect_zero(indi_du, INDI_NUM_ACT);
|
||||
}
|
||||
|
||||
// Propagate actuator filters
|
||||
get_actuator_state();
|
||||
for(i=0; i<INDI_NUM_ACT; i++) {
|
||||
@@ -701,7 +666,6 @@ static void rpm_cb(uint8_t __attribute__((unused)) sender_id, uint16_t UNUSED *r
|
||||
for(i=0; i<num_act; i++) {
|
||||
act_obs[i] = (rpm[i] - get_servo_min(i));
|
||||
act_obs[i] *= (MAX_PPRZ / (float)(get_servo_max(i)-get_servo_min(i)));
|
||||
Bound(act_obs[i], 0, MAX_PPRZ);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,27 +0,0 @@
|
||||
/*
|
||||
* This is part of the qr_solve library from John Burkardt.
|
||||
* http://people.sc.fsu.edu/~jburkardt/c_src/qr_solve/qr_solve.html
|
||||
*
|
||||
* It is slightly modified to make it compile on simple microprocessors,
|
||||
* and to remove all dynamic memory.
|
||||
*
|
||||
* This code is distributed under the GNU LGPL license.
|
||||
*/
|
||||
|
||||
void daxpy ( int n, float da, float dx[], int incx, float dy[], int incy );
|
||||
float ddot ( int n, float dx[], int incx, float dy[], int incy );
|
||||
float dnrm2 ( int n, float x[], int incx );
|
||||
void dqrank ( float a[], int lda, int m, int n, float tol, int *kr,
|
||||
int jpvt[], float qraux[] );
|
||||
void dqrdc ( float a[], int lda, int n, int p, float qraux[], int jpvt[],
|
||||
float work[], int job );
|
||||
int dqrls ( float a[], int lda, int m, int n, float tol, int *kr, float b[],
|
||||
float x[], float rsd[], int jpvt[], float qraux[], int itask );
|
||||
void dqrlss ( float a[], int lda, int m, int n, int kr, float b[], float x[],
|
||||
float rsd[], int jpvt[], float qraux[] );
|
||||
int dqrsl ( float a[], int lda, int n, int k, float qraux[], float y[],
|
||||
float qy[], float qty[], float b[], float rsd[], float ab[], int job );
|
||||
void drotg ( float *sa, float *sb, float *c, float *s );
|
||||
void dscal ( int n, float sa, float x[], int incx );
|
||||
void dswap ( int n, float x[], int incx, float y[], int incy );
|
||||
void qr_solve ( int m, int n, float a[], float b[], float x[] );
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,25 +0,0 @@
|
||||
/*
|
||||
* This file is a modified subset of the R8lib from John Burkardt.
|
||||
* http://people.sc.fsu.edu/~jburkardt/c_src/r8lib/r8lib.html
|
||||
*
|
||||
* It is the minimal set of functions from r8lib needed to use qr_solve.
|
||||
*
|
||||
* This code is distributed under the GNU LGPL license.
|
||||
*/
|
||||
|
||||
void r8mat_copy_new ( int m, int n, float a1[], float a2[] );
|
||||
float r8_epsilon ( void );
|
||||
float r8mat_amax ( int m, int n, float a[] );
|
||||
float r8_sign ( float x );
|
||||
float r8_max ( float x, float y );
|
||||
float *r8mat_transpose_new ( int m, int n, float a[] );
|
||||
float *r8mat_mm_new ( int n1, int n2, int n3, float a[], float b[] );
|
||||
float *r8mat_cholesky_factor ( int n, float a[], int *flag );
|
||||
float *r8mat_mv_new ( int m, int n, float a[], float x[] );
|
||||
float *r8mat_cholesky_solve ( int n, float l[], float b[] );
|
||||
float *r8mat_l_solve ( int n, float a[], float b[] );
|
||||
float *r8mat_lt_solve ( int n, float a[], float b[] );
|
||||
float *r8mat_mtv_new ( int m, int n, float a[], float x[] );
|
||||
float r8vec_max ( int n, float r8vec[] );
|
||||
int i4_min ( int i1, int i2 );
|
||||
int i4_max ( int i1, int i2 );
|
||||
@@ -1,347 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) Anton Naruta && Daniel Hoppener
|
||||
* MAVLab Delft University of Technology
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
/** @file wls_alloc.c
|
||||
* @brief 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. Submitted to IMAV 2017
|
||||
*
|
||||
* written by Anton Naruta && Daniel Hoppener 2016
|
||||
* MAVLab Delft University of Technology
|
||||
*/
|
||||
|
||||
#include "wls_alloc.h"
|
||||
#include <stdio.h>
|
||||
#include "std.h"
|
||||
|
||||
void print_final_values(int n_u, int n_v, float* u, float** B, float* v, float* umin, float* umax);
|
||||
void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, float* p_free);
|
||||
|
||||
// provide loop feedback
|
||||
#define WLS_VERBOSE FALSE
|
||||
|
||||
// the wrapper can use any solver function
|
||||
void qr_solve_wrapper(int m, int n, float** A, float* b, float* x) {
|
||||
float in[m * n];
|
||||
// convert A to 1d array
|
||||
int k = 0;
|
||||
for (int j = 0; j < n; j++) {
|
||||
for (int i = 0; i < m; i++) {
|
||||
in[k++] = A[i][j];
|
||||
}
|
||||
}
|
||||
// use solver
|
||||
qr_solve(m, n, in, b, x);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 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
|
||||
*
|
||||
* @param u The control output vector
|
||||
* @param v The control objective
|
||||
* @param umin The minimum u vector
|
||||
* @param umax The maximum u vector
|
||||
* @param B The control effectiveness matrix
|
||||
* @param n_u Length of u
|
||||
* @param n_v Lenght of v
|
||||
* @param u_guess Initial value for u
|
||||
* @param W_init Initial working set, if known
|
||||
* @param Wv Weighting on different control objectives
|
||||
* @param Wu Weighting on different controls
|
||||
* @param up Preferred control vector
|
||||
* @param gamma_sq Preference of satisfying control objective over desired
|
||||
* control vector (sqare root of gamma)
|
||||
* @param imax Max number of iterations
|
||||
*
|
||||
* @return Number of iterations, -1 upon failure
|
||||
*/
|
||||
int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
|
||||
int n_u, int n_v, float* u_guess, float* W_init, float* Wv,
|
||||
float* Wu, float* up, float gamma_sq, int imax) {
|
||||
// allocate variables, use defaults where parameters are set to 0
|
||||
if(!gamma_sq) gamma_sq = 100000;
|
||||
if(!imax) imax = 100;
|
||||
int n_c = n_u + n_v;
|
||||
|
||||
float A[n_c][n_u];
|
||||
float A_free[n_c][n_u];
|
||||
|
||||
// Create a pointer array to the rows of A_free
|
||||
// such that we can pass it to a function
|
||||
float * A_free_ptr[n_c];
|
||||
for(int i = 0; i < n_c; i++)
|
||||
A_free_ptr[i] = A_free[i];
|
||||
|
||||
float b[n_c];
|
||||
float d[n_c];
|
||||
|
||||
int free_index[n_u];
|
||||
int free_index_lookup[n_u];
|
||||
int n_free = 0;
|
||||
int free_chk = -1;
|
||||
|
||||
int iter = 0;
|
||||
float p_free[n_u];
|
||||
float p[n_u];
|
||||
float u_opt[n_u];
|
||||
int infeasible_index[n_u] UNUSED;
|
||||
int n_infeasible = 0;
|
||||
float lambda[n_u];
|
||||
float W[n_u];
|
||||
|
||||
// Initialize u and the working set, if provided from input
|
||||
if (!u_guess) {
|
||||
for (int i = 0; i < n_u; i++) {
|
||||
u[i] = (umax[i] + umin[i]) * 0.5;
|
||||
}
|
||||
} else {
|
||||
for (int i = 0; i < n_u; i++) {
|
||||
u[i] = u_guess[i];
|
||||
}
|
||||
}
|
||||
W_init ? memcpy(W, W_init, n_u * sizeof(float))
|
||||
: memset(W, 0, n_u * sizeof(float));
|
||||
|
||||
memset(free_index_lookup, -1, n_u * sizeof(float));
|
||||
|
||||
|
||||
// find free indices
|
||||
for (int i = 0; i < n_u; i++) {
|
||||
if (W[i] == 0) {
|
||||
free_index_lookup[i] = n_free;
|
||||
free_index[n_free++] = i;
|
||||
}
|
||||
}
|
||||
|
||||
// fill up A, A_free, b and d
|
||||
for (int i = 0; i < n_v; i++) {
|
||||
// If Wv is a NULL pointer, use Wv = identity
|
||||
b[i] = Wv ? gamma_sq * Wv[i] * v[i] : gamma_sq * v[i];
|
||||
d[i] = b[i];
|
||||
for (int j = 0; j < n_u; j++) {
|
||||
// If Wv is a NULL pointer, use Wv = identity
|
||||
A[i][j] = Wv ? gamma_sq * Wv[i] * B[i][j] : gamma_sq * B[i][j];
|
||||
d[i] -= A[i][j] * u[j];
|
||||
}
|
||||
}
|
||||
for (int i = n_v; i < n_c; i++) {
|
||||
memset(A[i], 0, n_u * sizeof(float));
|
||||
A[i][i - n_v] = Wu ? Wu[i - n_v] : 1.0;
|
||||
b[i] = up ? (Wu ? Wu[i] * up[i] : up[i]) : 0;
|
||||
d[i] = b[i] - A[i][i - n_v] * u[i - n_v];
|
||||
}
|
||||
|
||||
// -------------- Start loop ------------
|
||||
while (iter++ < imax) {
|
||||
// clear p, copy u to u_opt
|
||||
memset(p, 0, n_u * sizeof(float));
|
||||
memcpy(u_opt, u, n_u * sizeof(float));
|
||||
|
||||
// Construct a matrix with the free columns of A
|
||||
if (free_chk != n_free) {
|
||||
for (int i = 0; i < n_c; i++) {
|
||||
for (int j = 0; j < n_free; j++) {
|
||||
A_free[i][j] = A[i][free_index[j]];
|
||||
}
|
||||
}
|
||||
free_chk = n_free;
|
||||
}
|
||||
|
||||
|
||||
if (n_free) {
|
||||
// Still free variables left, calculate corresponding solution
|
||||
|
||||
// use a solver to find the solution to A_free*p_free = d
|
||||
qr_solve_wrapper(n_c, n_free, A_free_ptr, d, p_free);
|
||||
|
||||
//print results current step
|
||||
#if WLS_VERBOSE
|
||||
print_in_and_outputs(n_c, n_free, A_free_ptr, d, p_free);
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
// Set the nonzero values of p and add to u_opt
|
||||
for (int i = 0; i < n_free; i++) {
|
||||
p[free_index[i]] = p_free[i];
|
||||
u_opt[free_index[i]] += p_free[i];
|
||||
}
|
||||
// check limits
|
||||
n_infeasible = 0;
|
||||
for (int i = 0; i < n_u; i++) {
|
||||
if (u_opt[i] >= (umax[i] + 1.0) || u_opt[i] <= (umin[i] - 1.0)) {
|
||||
infeasible_index[n_infeasible++] = i;
|
||||
}
|
||||
}
|
||||
|
||||
// Check feasibility of the solution
|
||||
if (n_infeasible == 0) {
|
||||
// all variables are within limits
|
||||
memcpy(u, u_opt, n_u * sizeof(float));
|
||||
memset(lambda, 0, n_u * sizeof(float));
|
||||
|
||||
// d = d + A_free*p_free; lambda = A*d;
|
||||
for (int i = 0; i < n_c; i++) {
|
||||
for (int k = 0; k < n_free; k++) {
|
||||
d[i] -= A_free[i][k] * p_free[k];
|
||||
}
|
||||
for (int k = 0; k < n_u; k++) {
|
||||
lambda[k] += A[i][k] * d[i];
|
||||
}
|
||||
}
|
||||
bool break_flag = true;
|
||||
|
||||
// lambda = lambda x W;
|
||||
for (int i = 0; i < n_u; i++) {
|
||||
lambda[i] *= W[i];
|
||||
// if any lambdas are negative, keep looking for solution
|
||||
if (lambda[i] < -FLT_EPSILON) {
|
||||
break_flag = false;
|
||||
W[i] = 0;
|
||||
// add a free index
|
||||
if (free_index_lookup[i] < 0) {
|
||||
free_index_lookup[i] = n_free;
|
||||
free_index[n_free++] = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (break_flag) {
|
||||
|
||||
#if WLS_VERBOSE
|
||||
print_final_values(1, n_u, n_v, u, B, v, umin, umax);
|
||||
#endif
|
||||
|
||||
// if solution is found, return number of iterations
|
||||
return iter;
|
||||
}
|
||||
} else {
|
||||
float alpha = INFINITY;
|
||||
float alpha_tmp;
|
||||
int id_alpha = 0;
|
||||
|
||||
// find the lowest distance from the limit among the free variables
|
||||
for (int i = 0; i < n_free; i++) {
|
||||
int id = free_index[i];
|
||||
if(fabs(p[id]) > FLT_EPSILON) {
|
||||
alpha_tmp = (p[id] < 0) ? (umin[id] - u[id]) / p[id]
|
||||
: (umax[id] - u[id]) / p[id];
|
||||
} else {
|
||||
alpha_tmp = INFINITY;
|
||||
}
|
||||
if (alpha_tmp < alpha) {
|
||||
alpha = alpha_tmp;
|
||||
id_alpha = id;
|
||||
}
|
||||
}
|
||||
|
||||
// update input u = u + alpha*p
|
||||
for (int i = 0; i < n_u; i++) {
|
||||
u[i] += alpha * p[i];
|
||||
}
|
||||
// update d = d-alpha*A*p_free
|
||||
for (int i = 0; i < n_c; i++) {
|
||||
for (int k = 0; k < n_free; k++) {
|
||||
d[i] -= A_free[i][k] * alpha * p_free[k];
|
||||
}
|
||||
}
|
||||
// get rid of a free index
|
||||
W[id_alpha] = (p[id_alpha] > 0) ? 1.0 : -1.0;
|
||||
|
||||
free_index[free_index_lookup[id_alpha]] = free_index[--n_free];
|
||||
free_index_lookup[free_index[free_index_lookup[id_alpha]]] =
|
||||
free_index_lookup[id_alpha];
|
||||
free_index_lookup[id_alpha] = -1;
|
||||
}
|
||||
}
|
||||
// solution failed, return negative one to indicate failure
|
||||
return -1;
|
||||
}
|
||||
|
||||
void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, float* p_free) {
|
||||
|
||||
printf("n_c = %d n_free = %d\n", n_c, n_free);
|
||||
|
||||
printf("A_free =\n");
|
||||
for(int i = 0; i < n_c; i++) {
|
||||
for (int j = 0; j < n_free; j++) {
|
||||
printf("%f ", A_free_ptr[i][j]);
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
printf("d = ");
|
||||
for (int j = 0; j < n_c; j++) {
|
||||
printf("%f ", d[j]);
|
||||
}
|
||||
|
||||
printf("\noutput = ");
|
||||
for (int j = 0; j < n_free; j++) {
|
||||
printf("%f ", p_free[j]);
|
||||
}
|
||||
printf("\n\n");
|
||||
}
|
||||
|
||||
void print_final_values(int n_u, int n_v, float* u, float** B, float* v, float* umin, float* umax) {
|
||||
printf("n_u = %d n_v = %d\n", n_u, n_v);
|
||||
|
||||
printf("B =\n");
|
||||
for(int i = 0; i < n_v; i++) {
|
||||
for (int j = 0; j < n_u; j++) {
|
||||
printf("%f ", B[i][j]);
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
printf("v = ");
|
||||
for (int j = 0; j < n_v; j++) {
|
||||
printf("%f ", v[j]);
|
||||
}
|
||||
|
||||
printf("\nu = ");
|
||||
for (int j = 0; j < n_u; j++) {
|
||||
printf("%f ", u[j]);
|
||||
}
|
||||
printf("\n");
|
||||
|
||||
printf("\numin = ");
|
||||
for (int j = 0; j < n_u; j++) {
|
||||
printf("%f ", umin[j]);
|
||||
}
|
||||
printf("\n");
|
||||
|
||||
printf("\numax = ");
|
||||
for (int j = 0; j < n_u; j++) {
|
||||
printf("%f ", umax[j]);
|
||||
}
|
||||
printf("\n\n");
|
||||
|
||||
}
|
||||
@@ -1,21 +0,0 @@
|
||||
|
||||
//#include <stdio.h>
|
||||
//#include <stdlib>
|
||||
//#include <iostream>
|
||||
//#include <cstring>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <math.h>
|
||||
#include <float.h>
|
||||
#include "qr_solve.h"
|
||||
#include "r8lib_min.h"
|
||||
#ifndef DBL_EPSILON
|
||||
#define DBL_EPSILON 2.2204460492503131e-16
|
||||
#endif
|
||||
|
||||
void qr_solve_wrapper(int m, int n, float** A, float* b, float* x);
|
||||
|
||||
int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
|
||||
int n_u, int n_w, float* u_guess, float* W_init, float* Wv,
|
||||
float* Wu, float* ud, float gamma, int imax);
|
||||
@@ -21,11 +21,8 @@ test_algebra: test_algebra.c ../math/pprz_trig_int.c ../math/pprz_algebra_int.c
|
||||
test_bla: test_bla.c ../math/pprz_trig_int.c ../math/pprz_algebra_int.c ../math/pprz_algebra_float.c ../math/pprz_algebra_double.c
|
||||
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
|
||||
|
||||
test_alloc: test_alloc.c ../firmwares/rotorcraft/stabilization/wls/wls_alloc.c ../firmwares/rotorcraft/stabilization/wls/r8lib_min.c ../firmwares/rotorcraft/stabilization/wls/qr_solve.c
|
||||
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
|
||||
|
||||
%.exe : %.c
|
||||
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
|
||||
|
||||
clean:
|
||||
$(Q)rm -f *~ test_matrix test_geodetic test_algebra test_bla test_alloc *.exe
|
||||
$(Q)rm -f *~ test_matrix test_geodetic test_algebra test_bla *.exe
|
||||
|
||||
@@ -1,46 +0,0 @@
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "std.h"
|
||||
|
||||
#include "math/pprz_algebra_float.h"
|
||||
#include "math/pprz_algebra_double.h"
|
||||
#include "math/pprz_algebra_int.h"
|
||||
#include "pprz_algebra_print.h"
|
||||
#include "firmwares/rotorcraft/stabilization/wls/wls_alloc.h"
|
||||
|
||||
#define INDI_OUTPUTS 4
|
||||
#define INDI_NUM_ACT 4
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
float u_min[4] = {-107, -19093, 0, -95, };
|
||||
float u_max[4] = {19093, 107, 4600, 4505, };
|
||||
|
||||
float g1g2[4][4] =
|
||||
{{ 0, 0, -0.0105, 0.0107016},
|
||||
{-0.0030044, 0.0030044, 0.035, 0.035},
|
||||
{-0.004856, -0.004856, 0, 0},
|
||||
{ 0, 0, -0.0011, -0.0011}};
|
||||
|
||||
//State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST}
|
||||
static float Wv[INDI_OUTPUTS] = {100, 1000, 0.1, 10};
|
||||
/*static float Wv[INDI_OUTPUTS] = {10, 10, 0.1, 1};*/
|
||||
|
||||
// The control objective in array format
|
||||
float indi_v[4] = {10.8487, -10.5658, 6.8383, 1.8532};
|
||||
float indi_du[4];
|
||||
|
||||
// Initialize the array of pointers to the rows of g1g2
|
||||
float* Bwls[4];
|
||||
uint8_t i;
|
||||
for(i=0; i<INDI_OUTPUTS; i++) {
|
||||
Bwls[i] = g1g2[i];
|
||||
}
|
||||
|
||||
// WLS Control Allocator
|
||||
int num_iter =
|
||||
wls_alloc(indi_du,indi_v,u_min,u_max,Bwls,INDI_NUM_ACT,INDI_OUTPUTS,0,0,Wv,0,0,10000,10);
|
||||
printf("du = %f, %f, %f, %f\n", indi_du[0],indi_du[1],indi_du[2],indi_du[3]);
|
||||
}
|
||||
Reference in New Issue
Block a user