Skip to content

Commit

Permalink
Default behaviour is WLS
Browse files Browse the repository at this point in the history
  • Loading branch information
@dewagter
dewagter committed Sep 26, 2023
1 parent bd3e8d0 commit 46633ed
Showing 1 changed file with 74 additions and 17 deletions.
91 changes: 74 additions & 17 deletions sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,8 +66,9 @@
#define STABILIZATION_INDI_FILT_CUTOFF_R 20.0
#endif

// Default is WLS
#ifndef STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
#define STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE TRUE
#define STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE FALSE
#endif

#ifndef INDI_FILTER_RATES_SECOND_ORDER
Expand All @@ -80,6 +81,12 @@
#define INDI_HROTTLE_LIMIT_AIRSPEED_FWD 8.0
#endif

#if INDI_OUTPUTS > 4
#ifndef STABILIZATION_INDI_G1_THRUST_X
#error "You must define STABILIZATION_INDI_G1_THRUST_X for your number of INDI_OUTPUTS"
#endif
#endif

float du_min[INDI_NUM_ACT];
float du_max[INDI_NUM_ACT];
float du_pref[INDI_NUM_ACT];
Expand Down Expand Up @@ -126,6 +133,14 @@ 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_IS_THRUSTER_X
bool act_is_thruster_x[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_THRUSTER_X;
#else
bool act_is_thruster_x[INDI_NUM_ACT] = {0};
#endif

bool act_is_thruster_z[INDI_NUM_ACT];

#ifdef STABILIZATION_INDI_ACT_PREF
// Preferred (neutral, least energy) actuator value
float act_pref[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_PREF;
Expand All @@ -140,7 +155,11 @@ float act_dyn[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN;
static float Wv[INDI_OUTPUTS] = STABILIZATION_INDI_WLS_PRIORITIES;
#else
//State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST}
static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100};
#if INDI_OUTPUTS == 5
static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100, 100};
#else
static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100};
#endif
#endif

/**
Expand Down Expand Up @@ -185,6 +204,7 @@ float act_obs[INDI_NUM_ACT];

// Number of actuators used to provide thrust
int32_t num_thrusters;
int32_t num_thrusters_x;

struct Int32Eulers stab_att_sp_euler;
struct Int32Quat stab_att_sp_quat;
Expand All @@ -193,15 +213,23 @@ 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];
float g2[INDI_NUM_ACT] = STABILIZATION_INDI_G2; //scaled by INDI_G_SCALING
#if INDI_OUTPUTS == 5
float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW,
STABILIZATION_INDI_G1_THRUST, STABILIZATION_INDI_G1_THRUST_X
};
#else
float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST
};
#endif

float g1g2[INDI_OUTPUTS][INDI_NUM_ACT];
float g1_est[INDI_OUTPUTS][INDI_NUM_ACT];
float g2_est[INDI_NUM_ACT];
Expand All @@ -227,10 +255,10 @@ void sum_g1_g2(void);
#include "modules/datalink/telemetry.h"
static void send_indi_g(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_INDI_G(trans, dev, AC_ID, INDI_NUM_ACT, g1_est[0],
INDI_NUM_ACT, g1_est[1],
INDI_NUM_ACT, g1_est[2],
INDI_NUM_ACT, g1_est[3],
pprz_msg_send_INDI_G(trans, dev, AC_ID, INDI_NUM_ACT, g1g2[0],
INDI_NUM_ACT, g1g2[1],
INDI_NUM_ACT, g1g2[2],
INDI_NUM_ACT, g1g2[3],
INDI_NUM_ACT, g2_est);
}

Expand Down Expand Up @@ -294,10 +322,12 @@ void stabilization_indi_init(void)
float_vect_zero(estimation_rate_dd, INDI_NUM_ACT);
float_vect_zero(actuator_state_filt_vect, INDI_NUM_ACT);

//Calculate G1G2_PSEUDO_INVERSE
//Calculate G1G2
sum_g1_g2();

// Do not compute if not needed
#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
//Calculate G1G2_PSEUDO_INVERSE
calc_g1g2_pseudo_inv();
#endif

Expand All @@ -316,8 +346,14 @@ void stabilization_indi_init(void)

// Assume all non-servos are delivering thrust
num_thrusters = INDI_NUM_ACT;
num_thrusters_x = 0;
for (i = 0; i < INDI_NUM_ACT; i++) {
num_thrusters -= act_is_servo[i];
num_thrusters -= act_is_thruster_x[i];

num_thrusters_x += act_is_thruster_x[i];

act_is_thruster_z[i] = !act_is_servo[i] && !act_is_thruster_x[i];
}

#if PERIODIC_TELEMETRY
Expand Down Expand Up @@ -532,30 +568,48 @@ 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;

//update thrust command such that the current is correctly estimated
stabilization_cmd[COMMAND_THRUST] = 0;
for (i = 0; i < INDI_NUM_ACT; i++) {
stabilization_cmd[COMMAND_THRUST] += actuator_state[i] * -((int32_t) act_is_servo[i] - 1);
stabilization_cmd[COMMAND_THRUST] += actuator_state[i] * (int32_t) act_is_thruster_z[i];
}
stabilization_cmd[COMMAND_THRUST] /= num_thrusters;

#if INDI_OUTPUTS == 5
stabilization_cmd[COMMAND_THRUST_X] = 0;
for (i = 0; i < INDI_NUM_ACT; i++) {
stabilization_cmd[COMMAND_THRUST_X] += actuator_state[i] * (int32_t) act_is_thruster_x[i];
}
stabilization_cmd[COMMAND_THRUST_X] /= num_thrusters_x;
#endif

} 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];
#if INDI_OUTPUTS == 5
v_thrust.x +=
(stabilization_cmd[COMMAND_THRUST_X] - use_increment*actuator_state_filt_vect[i]) * Bwls[4][i];
#endif
}
}

// The control objective in array format
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 INDI_OUTPUTS == 5
indi_v[4] = v_thrust.x;
#endif

#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
// Calculate the increment for each actuator
Expand Down Expand Up @@ -587,9 +641,12 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
#endif
}

int16_t n_u = CA_N_U_INNER;
int16_t n_v = CA_N_V_INNER;

// 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, du_max, Bwls, 0, 0, Wv, indi_Wu, du_pref, 10000, 10, n_u, n_v);
#endif

if (in_flight) {
Expand Down Expand Up @@ -920,12 +977,12 @@ void calc_g1g2_pseudo_inv(void)
}
#endif

static void rpm_cb(uint8_t sender_id UNUSED, struct rpm_act_t *rpm_msg UNUSED, uint8_t num_act UNUSED)
static void rpm_cb(uint8_t sender_id UNUSED, struct rpm_act_t *rpm_msg UNUSED, uint8_t num_act_msg UNUSED)
{
#if INDI_RPM_FEEDBACK
PRINT_CONFIG_MSG("INDI_RPM_FEEDBACK");
int8_t i;
for (i = 0; i < num_act; i++) {
for (i = 0; i < num_act_msg; i++) {
// Sanity check that index is valid
if (rpm_msg[i].actuator_idx < INDI_NUM_ACT) {
int8_t idx = rpm_msg[i].actuator_idx;
Expand All @@ -940,7 +997,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

0 comments on commit 46633ed

Please sign in to comment.