78 changes: 48 additions & 30 deletions sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
View file
Expand Up @@ -42,7 +42,7 @@
#include "modules/actuators/actuators.h"
#include "modules/core/abi.h"
#include "filters/low_pass_filter.h"
#include "wls/wls_alloc.h"
#include "math/wls/wls_alloc.h"
#include <stdio.h>

// Factor that the estimated G matrix is allowed to deviate from initial one
Expand Down Expand Up @@ -81,9 +81,13 @@
#define INDI_HROTTLE_LIMIT_AIRSPEED_FWD 8.0
#endif

float du_min[INDI_NUM_ACT];
float du_max[INDI_NUM_ACT];
float du_pref[INDI_NUM_ACT];
#ifdef SetCommandsFromRC
#warning SetAutoCommandsFromRC not used: STAB_INDI overwrites actuators
#endif

float du_min_stab_indi[INDI_NUM_ACT];
float du_max_stab_indi[INDI_NUM_ACT];
float du_pref_stab_indi[INDI_NUM_ACT];
float indi_v[INDI_OUTPUTS];
float *Bwls[INDI_OUTPUTS];
int num_iter = 0;
Expand Down Expand Up @@ -195,8 +199,8 @@ abi_event rpm_ev;
static void rpm_cb(uint8_t sender_id, struct rpm_act_t *rpm_msg, uint8_t num_act);

abi_event thrust_ev;
static void thrust_cb(uint8_t sender_id, float thrust_increment);
float indi_thrust_increment;
static void thrust_cb(uint8_t sender_id, struct FloatVect3 thrust_increment);
struct FloatVect3 indi_thrust_increment;
bool indi_thrust_increment_set = false;

float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS];
Expand Down Expand Up @@ -540,7 +544,10 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
use_increment = 1.0;
}

float v_thrust = 0.0;
struct FloatVect3 v_thrust;
v_thrust.x = 0.0;
v_thrust.y = 0.0;
v_thrust.z = 0.0;
if (indi_thrust_increment_set) {
v_thrust = indi_thrust_increment;

Expand All @@ -554,7 +561,7 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
} else {
// incremental thrust
for (i = 0; i < INDI_NUM_ACT; i++) {
v_thrust +=
v_thrust.z +=
(stabilization_cmd[COMMAND_THRUST] - use_increment*actuator_state_filt_vect[i]) * Bwls[3][i];
}
}
Expand All @@ -563,7 +570,7 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
indi_v[0] = (angular_accel_ref.p - use_increment*angular_acceleration[0]);
indi_v[1] = (angular_accel_ref.q - use_increment*angular_acceleration[1]);
indi_v[2] = (angular_accel_ref.r - use_increment*angular_acceleration[2] + g2_times_du);
indi_v[3] = v_thrust;
indi_v[3] = v_thrust.z;

#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
// Calculate the increment for each actuator
Expand All @@ -574,30 +581,12 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
+ (g1g2_pseudo_inv[i][3] * indi_v[3]);
}
#else
// Calculate the min and max increments
for (i = 0; i < INDI_NUM_ACT; i++) {
du_min[i] = -MAX_PPRZ * act_is_servo[i] - use_increment*actuator_state_filt_vect[i];
du_max[i] = MAX_PPRZ - use_increment*actuator_state_filt_vect[i];
du_pref[i] = act_pref[i] - use_increment*actuator_state_filt_vect[i];
stabilization_indi_set_wls_settings(use_increment);

#ifdef GUIDANCE_INDI_MIN_THROTTLE
float airspeed = stateGetAirspeed_f();
//limit minimum thrust ap can give
if (!act_is_servo[i]) {
if ((guidance_h.mode == GUIDANCE_H_MODE_HOVER) || (guidance_h.mode == GUIDANCE_H_MODE_NAV)) {
if (airspeed < INDI_HROTTLE_LIMIT_AIRSPEED_FWD) {
du_min[i] = GUIDANCE_INDI_MIN_THROTTLE - use_increment*actuator_state_filt_vect[i];
} else {
du_min[i] = GUIDANCE_INDI_MIN_THROTTLE_FWD - use_increment*actuator_state_filt_vect[i];
}
}
}
#endif
}

// WLS Control Allocator
num_iter =
wls_alloc(indi_du, indi_v, du_min, du_max, Bwls, 0, 0, Wv, indi_Wu, du_pref, 10000, 10);
wls_alloc(indi_du, indi_v, du_min_stab_indi, du_max_stab_indi, Bwls, 0, 0, Wv, indi_Wu, du_pref_stab_indi, 10000, 10);
#endif

if (in_flight) {
Expand Down Expand Up @@ -652,6 +641,35 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
stabilization_cmd[COMMAND_YAW] = 42;
}

/**
* @param use_increment
*
* Function that sets the du_min, du_max and du_pref if function not elsewhere defined
*/
void WEAK stabilization_indi_set_wls_settings(float use_increment)
{
// Calculate the min and max increments
for (uint8_t i = 0; i < INDI_NUM_ACT; i++) {
du_min_stab_indi[i] = -MAX_PPRZ * act_is_servo[i] - use_increment*actuator_state_filt_vect[i];
du_max_stab_indi[i] = MAX_PPRZ - use_increment*actuator_state_filt_vect[i];
du_pref_stab_indi[i] = act_pref[i] - use_increment*actuator_state_filt_vect[i];

#ifdef GUIDANCE_INDI_MIN_THROTTLE
float airspeed = stateGetAirspeed_f();
//limit minimum thrust ap can give
if (!act_is_servo[i]) {
if ((guidance_h.mode == GUIDANCE_H_MODE_HOVER) || (guidance_h.mode == GUIDANCE_H_MODE_NAV)) {
if (airspeed < INDI_HROTTLE_LIMIT_AIRSPEED_FWD) {
du_min_stab_indi[i] = GUIDANCE_INDI_MIN_THROTTLE - use_increment*actuator_state_filt_vect[i];
} else {
du_min_stab_indi[i] = GUIDANCE_INDI_MIN_THROTTLE_FWD - use_increment*actuator_state_filt_vect[i];
}
}
}
#endif
}
}

/**
* @param enable_integrator
* @param rate_control boolean that determines if we are in rate control or attitude control
Expand Down Expand Up @@ -951,7 +969,7 @@ PRINT_CONFIG_MSG("INDI_RPM_FEEDBACK");
/**
* ABI callback that obtains the thrust increment from guidance INDI
*/
static void thrust_cb(uint8_t UNUSED sender_id, float thrust_increment)
static void thrust_cb(uint8_t UNUSED sender_id, struct FloatVect3 thrust_increment)
{
indi_thrust_increment = thrust_increment;
indi_thrust_increment_set = true;
Expand Down
4 changes: 4 additions & 0 deletions sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h
View file
Expand Up @@ -37,6 +37,9 @@ extern float actuator_state_filt_vect[INDI_NUM_ACT];

extern bool indi_use_adaptive;

extern float du_min_stab_indi[INDI_NUM_ACT];
extern float du_max_stab_indi[INDI_NUM_ACT];
extern float du_pref_stab_indi[INDI_NUM_ACT];
extern float *Bwls[INDI_OUTPUTS];

extern float act_pref[INDI_NUM_ACT];
Expand All @@ -60,6 +63,7 @@ extern void stabilization_indi_set_quat_setpoint_i(struct Int32Quat *quat);
extern void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading);
extern void stabilization_indi_set_stab_sp(struct StabilizationSetpoint *sp);
extern void stabilization_indi_rate_run(struct FloatRates rate_ref, bool in_flight);
extern void stabilization_indi_set_wls_settings(float use_increment);
extern void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight);
extern void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn);

Expand Down
49 changes: 49 additions & 0 deletions sw/airborne/math/pprz_algebra_float.c
View file
Expand Up @@ -813,6 +813,55 @@ void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect
vect_out->y = mat[2] * vect_in.x + mat[3] * vect_in.y;
}

/**
* @brief 3x3 matrix inverse
*
* @param inv_out[3][3] inverted matrix output
* @param mat_in[3][3] matrix to be inverted
*
* @return success (0) or not invertible (1)
*/
bool float_mat_inv_3d(float inv_out[3][3], float mat_in[3][3])
{
const float m00 = mat_in[1][1]*mat_in[2][2] - mat_in[1][2]*mat_in[2][1];
const float m10 = mat_in[0][1]*mat_in[2][2] - mat_in[0][2]*mat_in[2][1];
const float m20 = mat_in[0][1]*mat_in[1][2] - mat_in[0][2]*mat_in[1][1];
const float m01 = mat_in[1][0]*mat_in[2][2] - mat_in[1][2]*mat_in[2][0];
const float m11 = mat_in[0][0]*mat_in[2][2] - mat_in[0][2]*mat_in[2][0];
const float m21 = mat_in[0][0]*mat_in[1][2] - mat_in[0][2]*mat_in[1][0];
const float m02 = mat_in[1][0]*mat_in[2][1] - mat_in[1][1]*mat_in[2][0];
const float m12 = mat_in[0][0]*mat_in[2][1] - mat_in[0][1]*mat_in[2][0];
const float m22 = mat_in[0][0]*mat_in[1][1] - mat_in[0][1]*mat_in[1][0];
const float det = mat_in[0][0]*m00 - mat_in[1][0]*m10 + mat_in[2][0]*m20;
if (fabs(det) > FLT_EPSILON) {
inv_out[0][0] = m00 / det;
inv_out[1][0] = -m01 / det;
inv_out[2][0] = m02 / det;
inv_out[0][1] = -m10 / det;
inv_out[1][1] = m11 / det;
inv_out[2][1] = -m12 / det;
inv_out[0][2] = m20 / det;
inv_out[1][2] = -m21 / det;
inv_out[2][2] = m22 / det;
return 0;
}
return 1;
}

/**
* @brief Multiply 3D matrix with vector
*
* @param vect_out output vector
* @param mat[3][3] Matrix input
* @param vect_in Vector input
*/
void float_mat3_mult(struct FloatVect3 *vect_out, float mat[3][3], struct FloatVect3 vect_in)
{
vect_out->x = mat[0][0] * vect_in.x + mat[0][1] * vect_in.y + mat[0][2] * vect_in.z;
vect_out->y = mat[1][0] * vect_in.x + mat[1][1] * vect_in.y + mat[1][2] * vect_in.z;
vect_out->z = mat[2][0] * vect_in.x + mat[2][1] * vect_in.y + mat[2][2] * vect_in.z;
}

/*
* 4x4 Matrix inverse.
* obtained from: http://rodolphe-vaillant.fr/?e=7
Expand Down
2 changes: 2 additions & 0 deletions sw/airborne/math/pprz_algebra_float.h
View file
Expand Up @@ -908,6 +908,8 @@ static inline void float_mat_mul_scalar(float **o, float **a, float scalar, int

extern bool float_mat_inv_2d(float inv_out[4], float mat_in[4]);
extern void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct FloatVect2 vect_in);
extern bool float_mat_inv_3d(float inv_out[3][3], float mat_in[3][3]);
extern void float_mat3_mult(struct FloatVect3 *vect_out, float mat[3][3], struct FloatVect3 vect_in);
extern bool float_mat_inv_4d(float invOut[4][4], float mat_in[4][4]);

extern void float_vect3_bound_in_2d(struct FloatVect3 *vect3, float bound);
Expand Down
76 changes: 40 additions & 36 deletions .../rotorcraft/stabilization/wls/wls_alloc.c → sw/airborne/math/wls/wls_alloc.c
View file
Expand Up @@ -36,7 +36,6 @@
*/

#include "wls_alloc.h"
#include <stdio.h>
#include "std.h"

#include <string.h>
Expand All @@ -45,22 +44,26 @@
#include "math/qr_solve/qr_solve.h"
#include "math/qr_solve/r8lib_min.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
#ifndef WLS_VERBOSE
#define WLS_VERBOSE FALSE
#endif

#if WLS_VERBOSE
#include <stdio.h>
static void print_final_values(int n_u, int n_v, float* u, float** B, float* v, float* umin, float* umax);
static void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, float* p_free);
#endif

// Problem size needs to be predefined to avoid having to use VLAs
#ifndef CA_N_V
#error CA_N_V needs to be defined!
#ifndef WLS_N_U
#define WLS_N_U 6
#endif

#ifndef CA_N_U
#error CA_N_U needs to be defined!
#ifndef WLS_N_V
#define WLS_N_V 4
#endif

#define CA_N_C (CA_N_U+CA_N_V)
#define WLS_N_C ((WLS_N_U)+(WLS_N_V))

/**
* @brief Wrapper for qr solve
Expand All @@ -71,7 +74,7 @@ void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, flo
* @param m number of rows
* @param n number of columns
*/
void qr_solve_wrapper(int m, int n, float** A, float* b, float* x) {
static 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;
Expand All @@ -92,7 +95,7 @@ void qr_solve_wrapper(int m, int n, float** A, float* b, float* x) {
* weighting matrices Wv and Wu
*
* @param u The control output vector
* @param v The control objective
* @param v The control objective vector
* @param umin The minimum u vector
* @param umax The maximum u vector
* @param B The control effectiveness matrix
Expand All @@ -107,44 +110,46 @@ void qr_solve_wrapper(int m, int n, float** A, float* b, float* x) {
* control vector (sqare root of gamma)
* @param imax Max number of iterations
*
* @return Number of iterations
* @return Number of iterations, -1 upon failure
*/
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 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)
{
// allocate variables, use defaults where parameters are set to 0
if(!gamma_sq) gamma_sq = 100000;
if(!imax) imax = 100;

int n_c = CA_N_C;
int n_u = CA_N_U;
int n_v = CA_N_V;
int n_u = WLS_N_U;
int n_v = WLS_N_V;
int n_c = n_u + n_v;

float A[CA_N_C][CA_N_U];
float A_free[CA_N_C][CA_N_U];
float A[WLS_N_C][WLS_N_U];
float A_free[WLS_N_C][WLS_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[CA_N_C];
float * A_free_ptr[WLS_N_C];
for(int i = 0; i < n_c; i++)
A_free_ptr[i] = A_free[i];

float b[CA_N_C];
float d[CA_N_C];
float b[WLS_N_C];
float d[WLS_N_C];

int free_index[CA_N_U];
int free_index_lookup[CA_N_U];
int free_index[WLS_N_U];
int free_index_lookup[WLS_N_U];
int n_free = 0;
int free_chk = -1;

int iter = 0;
float p_free[CA_N_U];
float p[CA_N_U];
float u_opt[CA_N_U];
int infeasible_index[CA_N_U] UNUSED;
float p_free[WLS_N_U];
float p[WLS_N_U];
float u_opt[WLS_N_U];
int infeasible_index[WLS_N_U] UNUSED;
int n_infeasible = 0;
float lambda[CA_N_U];
float W[CA_N_U];
float lambda[WLS_N_U];
float W[WLS_N_U];

// Initialize u and the working set, if provided from input
if (!u_guess) {
Expand All @@ -161,7 +166,6 @@ int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,

memset(free_index_lookup, -1, n_u * sizeof(float));


// find free indices
for (int i = 0; i < n_u; i++) {
if (W[i] == 0) {
Expand Down Expand Up @@ -311,11 +315,11 @@ int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
}
}
// solution failed, return negative one to indicate failure
return iter;
return -1;
}

#if WLS_VERBOSE
void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, float* p_free) {
static 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);

Expand All @@ -339,7 +343,7 @@ void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, flo
printf("\n\n");
}

void print_final_values(int n_u, int n_v, float* u, float** B, float* v, float* umin, float* umax) {
static 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");
Expand Down
19 changes: 6 additions & 13 deletions .../rotorcraft/stabilization/wls/wls_alloc.h → sw/airborne/math/wls/wls_alloc.h
View file
Expand Up @@ -20,26 +20,18 @@
* Boston, MA 02111-1307, USA.
*/

/**
* @brief Wrapper for qr solve
*
* Possible to use a different solver if needed.
* Solves a system of the form Ax = b for x.
*
* @param m number of rows
* @param n number of columns
*/
void qr_solve_wrapper(int m, int n, float** A, float* b, float* 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
*
* The dimension of the input vectors u and v are defined at compilation time
* and must be large enough for all the considered cases.
*
* @param u The control output vector
* @param v The control objective
* @param v The control objective vector
* @param umin The minimum u vector
* @param umax The maximum u vector
* @param B The control effectiveness matrix
Expand All @@ -56,6 +48,7 @@ void qr_solve_wrapper(int m, int n, float** A, float* b, float* x);
*
* @return Number of iterations, -1 upon failure
*/
int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
int wls_alloc(float* u, float* v,
float* umin, float* umax, float** B,
float* u_guess, float* W_init, float* Wv, float* Wu,
float* ud, float gamma, int imax);
4 changes: 2 additions & 2 deletions sw/airborne/test/Makefile
View file
Expand Up @@ -26,8 +26,8 @@ test_bla: test_bla.c ../math/pprz_trig_int.c ../math/pprz_algebra_int.c ../math/
test_geo: test_geo_conversions.c ../math/pprz_trig_int.c ../math/pprz_algebra_int.c ../math/pprz_algebra_float.c ../math/pprz_algebra_double.c ../math/pprz_geodetic_int.c ../math/pprz_geodetic_float.c ../math/pprz_geodetic_double.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)

test_alloc: test_alloc.c ../firmwares/rotorcraft/stabilization/wls/wls_alloc.c ../math/qr_solve/r8lib_min.c ../math/qr_solve/qr_solve.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
test_alloc: test_alloc.c ../math/wls/wls_alloc.c ../math/qr_solve/r8lib_min.c ../math/qr_solve/qr_solve.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS) -DWLS_N_U=8 -DWLS_N_V=4

test_tt: test_tilt_twist.c ../math/pprz_algebra_float.c
$(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/test/test_alloc.c
View file
Expand Up @@ -30,7 +30,7 @@

#include <stdio.h>
#include "std.h"
#include "firmwares/rotorcraft/stabilization/wls/wls_alloc.h"
#include "math/wls/wls_alloc.h"

#define INDI_OUTPUTS 4

Expand Down
3 changes: 2 additions & 1 deletion sw/include/std.h
View file
Expand Up @@ -135,8 +135,9 @@ typedef uint8_t unit_t;

#define BoundUpper(_x, _max) { if (_x > (_max)) _x = (_max);}

// Note: the bound function will bound NaN to min as any comparison that contains NaN is false.
#define Bound(_x, _min, _max) { if (!(_x > (_min))) _x = (_min); else if (!(_x < (_max))) _x = (_max); }

#define Bound(_x, _min, _max) { if (_x > (_max)) _x = (_max); else if (_x < (_min)) _x = (_min); }
#define BoundInverted(_x, _min, _max) { \
if ((_x < (_min)) && (_x > (_max))) { \
if (abs(_x - (_min)) < abs(_x - (_max))) \
Expand Down