Skip to content

Commit

Permalink
quadplane guidance (#3122)
Browse files Browse the repository at this point in the history 
* quadplane guidance
* Function doc and ifdef protect for normal guidance
  • Loading branch information
@dewagter
dewagter committed Oct 3, 2023
1 parent 8353c0a commit 19875f6
Show file tree
Hide file tree
Showing 4 changed files with 270 additions and 44 deletions.
224 changes: 180 additions & 44 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,11 +55,6 @@
#define GUIDANCE_INDI_POS_GAINZ 0.5
#endif

#ifndef GUIDANCE_INDI_MIN_PITCH
#define GUIDANCE_INDI_MIN_PITCH -120
#define GUIDANCE_INDI_MAX_PITCH 25
#endif

#ifndef GUIDANCE_INDI_LIFTD_ASQ
#define GUIDANCE_INDI_LIFTD_ASQ 0.20
#endif
Expand Down Expand Up @@ -91,13 +86,31 @@ struct guidance_indi_hybrid_params gih_params = {
#endif
float guidance_indi_max_airspeed = GUIDANCE_INDI_MAX_AIRSPEED;

// Quadplanes can hover at various pref pitch
float guidance_indi_pitch_pref_deg = 0;


#if GUIDANCE_INDI_QUADPLANE
#ifndef GUIDANCE_INDI_THRUST_Z_EFF
#error "You need to define GUIDANCE_INDI_THRUST_Z_EFF"
#else
float guidance_indi_thrust_z_eff = GUIDANCE_INDI_THRUST_Z_EFF;
#endif

#ifndef GUIDANCE_INDI_THRUST_X_EFF
#error "You need to define GUIDANCE_INDI_THRUST_X_EFF"
#else
float guidance_indi_thrust_x_eff = GUIDANCE_INDI_THRUST_X_EFF;
#endif
#endif

// If using WLS, check that the matrix size is sufficient
#if GUIDANCE_INDI_HYBRID_USE_WLS
#if 3 > WLS_N_U
#if GUIDANCE_INDI_HYBRID_U > WLS_N_U
#error Matrix-WLS_N_U too small: increase WLS_N_U in airframe file
#endif

#if 3 > WLS_N_V
#if GUIDANCE_INDI_HYBRID_V > WLS_N_V
#error Matrix-WLS_N_V too small: increase WLS_N_V in airframe file
#endif
#endif
Expand Down Expand Up @@ -168,31 +181,33 @@ Butterworth2LowPass roll_filt;
Butterworth2LowPass pitch_filt;
Butterworth2LowPass thrust_filt;
Butterworth2LowPass accely_filt;
Butterworth2LowPass accel_bodyz_filt;

struct FloatVect2 desired_airspeed;

float Ga[3][3];
float Ga[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U];
struct FloatVect3 euler_cmd;

#if GUIDANCE_INDI_HYBRID_USE_WLS
#include "math/wls/wls_alloc.h"
float du_min_gih[3];
float du_max_gih[3];
float du_pref_gih[3];
float *Bwls_gih[3];
float du_min_gih[GUIDANCE_INDI_HYBRID_U];
float du_max_gih[GUIDANCE_INDI_HYBRID_U];
float du_pref_gih[GUIDANCE_INDI_HYBRID_U];
float *Bwls_gih[GUIDANCE_INDI_HYBRID_V];
#ifdef GUIDANCE_INDI_HYBRID_WLS_PRIORITIES
float Wv_gih[3] = GUIDANCE_INDI_HYBRID_WLS_PRIORITIES;
float Wv_gih[GUIDANCE_INDI_HYBRID_V] = GUIDANCE_INDI_HYBRID_WLS_PRIORITIES;
#else
float Wv_gih[3] = { 100.f, 100.f, 1.f }; // X,Y accel, Z accel
float Wv_gih[GUIDANCE_INDI_HYBRID_V] = { 100.f, 100.f, 1.f }; // X,Y accel, Z accel
#endif
#ifdef GUIDANCE_INDI_HYBRID_WLS_WU
float Wu_gih[3] = GUIDANCE_INDI_HYBRID_WLS_WU;
float Wu_gih[GUIDANCE_INDI_HYBRID_U] = GUIDANCE_INDI_HYBRID_WLS_WU;
#else
float Wu_gih[3] = { 1.f, 1.f, 1.f };
float Wu_gih[GUIDANCE_INDI_HYBRID_U] = { 1.f, 1.f, 1.f };
#endif
#endif

float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF;
float bodyz_filter_cutoff = 0.2;

float guidance_indi_hybrid_heading_sp = 0.f;
struct FloatEulers guidance_euler_cmd;
Expand All @@ -209,7 +224,7 @@ struct FloatVect3 indi_vel_sp = {0.0, 0.0, 0.0};
float time_of_vel_sp = 0.0;

void guidance_indi_propagate_filters(void);
static void guidance_indi_calcg_wing(float Gmat[3][3]);
void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float v_body[GUIDANCE_INDI_HYBRID_V]);

#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
Expand Down Expand Up @@ -250,7 +265,7 @@ void guidance_indi_init(void)
init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);

#if GUIDANCE_INDI_HYBRID_USE_WLS
for (int8_t i = 0; i < 3; i++) {
for (int8_t i = 0; i < GUIDANCE_INDI_HYBRID_V; i++) {
Bwls_gih[i] = Ga[i];
}
#endif
Expand All @@ -274,6 +289,7 @@ void guidance_indi_enter(void) {
guidance_indi_hybrid_heading_sp = stateGetNedToBodyEulers_f()->psi;

float tau = 1.0 / (2.0 * M_PI * filter_cutoff);
float tau_bodyz = 1.0/(2.0*M_PI*bodyz_filter_cutoff);
float sample_time = 1.0 / PERIODIC_FREQUENCY;
for (int8_t i = 0; i < 3; i++) {
init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0);
Expand All @@ -286,6 +302,7 @@ void guidance_indi_enter(void) {
init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, eulers_zxy.theta);
init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, thrust_in);
init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);
init_butterworth_2_low_pass(&accel_bodyz_filt, tau_bodyz, sample_time, -9.81);
}

#include "firmwares/rotorcraft/navigation.h"
Expand Down Expand Up @@ -350,30 +367,25 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
#endif
#endif

// Calculate matrix of partial derivatives
guidance_indi_calcg_wing(Ga);

// Create the control objective
float v_gih[3];

// Calculate matrix of partial derivatives and control objective
guidance_indi_calcg_wing(Ga, a_diff, v_gih);

#if GUIDANCE_INDI_HYBRID_USE_WLS
// Set lower limits
du_min_gih[0] = -guidance_indi_max_bank - roll_filt.o[0]; // roll
du_min_gih[1] = RadOfDeg(guidance_indi_min_pitch) - pitch_filt.o[0]; // pitch
du_min_gih[2] = (MAX_PPRZ - actuator_state_filt_vect[0]) * g1g2[3][0] + (MAX_PPRZ - actuator_state_filt_vect[1]) * g1g2[3][1] + (MAX_PPRZ - actuator_state_filt_vect[2]) * g1g2[3][2] + (MAX_PPRZ - actuator_state_filt_vect[3]) * g1g2[3][3];

// Set upper limits limits
du_max_gih[0] = guidance_indi_max_bank - roll_filt.o[0]; //roll
du_max_gih[1] = RadOfDeg(GUIDANCE_INDI_MAX_PITCH) - pitch_filt.o[0]; // pitch
du_max_gih[2] = -(actuator_state_filt_vect[0]*g1g2[3][0] + actuator_state_filt_vect[1]*g1g2[3][1] + actuator_state_filt_vect[2]*g1g2[3][2] + actuator_state_filt_vect[3]*g1g2[3][3]);
// Calculate the maximum deflections
guidance_indi_hybrid_set_wls_settings(v_gih, roll_filt.o[0], pitch_filt.o[0]);

// Set prefered states
du_pref_gih[0] = -roll_filt.o[0]; // prefered delta roll angle
du_pref_gih[1] = -pitch_filt.o[0]; // prefered delta pitch angle
du_pref_gih[2] = du_max_gih[2];
float du_gih[GUIDANCE_INDI_HYBRID_U]; // = {0.0f, 0.0f, 0.0f};

float v_gih[3] = { a_diff.x, a_diff.y, a_diff.z };
float du_gih[3];
int num_iter UNUSED = wls_alloc(
du_gih, v_gih, du_min_gih, du_max_gih,
Bwls_gih, 0, 0, Wv_gih, Wu_gih, du_pref_gih, 100000, 10, 3, 3);
Bwls_gih, 0, 0, Wv_gih, Wu_gih, du_pref_gih, 100000, 10,
GUIDANCE_INDI_HYBRID_U, GUIDANCE_INDI_HYBRID_V);

euler_cmd.x = du_gih[0];
euler_cmd.y = du_gih[1];
euler_cmd.z = du_gih[2];
Expand All @@ -387,8 +399,12 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
#endif

struct FloatVect3 thrust_vect;
thrust_vect.x = 0.0; // Fill for quadplanes
thrust_vect.y = 0.0;
#if GUIDANCE_INDI_HYBRID_U > 3
thrust_vect.x = du_gih[3];
#else
thrust_vect.x = 0;
#endif
thrust_vect.y = 0;
thrust_vect.z = euler_cmd.z;
AbiSendMsgTHRUST(THRUST_INCREMENT_ID, thrust_vect);

Expand Down Expand Up @@ -686,7 +702,6 @@ struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight UNUSED, struc
}
}

#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
/**
* Filter the thrust, such that it corresponds to the filtered acceleration
*/
Expand All @@ -698,7 +713,6 @@ void guidance_indi_filter_thrust(void)
// same filter as for the acceleration
update_butterworth_2_low_pass(&thrust_filt, thrust_act);
}
#endif

/**
* Low pass the accelerometer measurements to remove noise from vibrations.
Expand All @@ -718,16 +732,23 @@ void guidance_indi_propagate_filters(void) {
// Propagate filter for sideslip correction
float accely = ACCEL_FLOAT_OF_BFP(stateGetAccelBody_i()->y);
update_butterworth_2_low_pass(&accely_filt, accely);

// Propagate filter for thrust/lift estimate
float accelz = ACCEL_FLOAT_OF_BFP(stateGetAccelBody_i()->z);
update_butterworth_2_low_pass(&accel_bodyz_filt, accelz);
}

/**
* Calculate the matrix of partial derivatives of the roll, pitch and thrust
* w.r.t. the NED accelerations, taking into account the lift of a wing that is
* horizontal at -90 degrees pitch
*
* @param Gmat array to write the matrix to [3x3]
* @param Gmat Dynamics matrix
* @param a_diff acceleration errors in earth frame
* @param body_v 3D vector to write the control objective v
*/
void guidance_indi_calcg_wing(float Gmat[3][3]) {
#if GUIDANCE_INDI_HYBRID_USE_WLS
void WEAK guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float v_gih[GUIDANCE_INDI_HYBRID_V]) {

/*Pre-calculate sines and cosines*/
float sphi = sinf(eulers_zxy.phi);
Expand Down Expand Up @@ -760,8 +781,123 @@ void guidance_indi_calcg_wing(float Gmat[3][3]) {
Gmat[0][2] = stheta*cpsi + sphi*ctheta*spsi;
Gmat[1][2] = stheta*spsi - sphi*ctheta*cpsi;
Gmat[2][2] = cphi*ctheta;

v_gih[0] = a_diff.x;
v_gih[1] = a_diff.y;
v_gih[2] = a_diff.z;
}
#elif defined(GUIDANCE_INDI_QUADPLANE)
/**
* Perform WLS
*
* @param Gmat Dynamics matrix
* @param a_diff acceleration errors in earth frame
* @param body_v 3D vector to write the control objective v
*/
void WEAK guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U], struct FloatVect3 a_diff, float body_v[GUIDANCE_INDI_HYBRID_V]) {
/*Pre-calculate sines and cosines*/
float sphi = sinf(eulers_zxy.phi);
float cphi = cosf(eulers_zxy.phi);
float stheta = sinf(eulers_zxy.theta);
float ctheta = cosf(eulers_zxy.theta);
float spsi = sinf(eulers_zxy.psi);
float cpsi = cosf(eulers_zxy.psi);

#ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
#define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0
#endif

/*Amount of lift produced by the wing*/
float lift_thrust_bz = accel_bodyz_filt.o[0]; // Sum of lift and thrust in boxy z axis (level flight)

// get the derivative of the lift wrt to theta
float liftd = guidance_indi_get_liftd(0.0f, 0.0f);

Gmat[0][0] = -sphi*stheta*lift_thrust_bz;
Gmat[1][0] = -cphi*lift_thrust_bz;
Gmat[2][0] = -sphi*ctheta*lift_thrust_bz;

Gmat[0][1] = cphi*ctheta*lift_thrust_bz*GUIDANCE_INDI_PITCH_EFF_SCALING;
Gmat[1][1] = sphi*stheta*lift_thrust_bz*GUIDANCE_INDI_PITCH_EFF_SCALING - sphi*liftd;
Gmat[2][1] = -cphi*stheta*lift_thrust_bz*GUIDANCE_INDI_PITCH_EFF_SCALING + cphi*liftd;

Gmat[0][2] = cphi*stheta;
Gmat[1][2] = -sphi;
Gmat[2][2] = cphi*ctheta;

Gmat[0][3] = ctheta;
Gmat[1][3] = 0;
Gmat[2][3] = -stheta;

// Convert acceleration error to body axis system
body_v[0] = cpsi * a_diff.x + spsi * a_diff.y;
body_v[1] = -spsi * a_diff.x + cpsi * a_diff.y;
body_v[2] = a_diff.z;
}

#else

void WEAK guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_HYBRID_U] UNUSED, struct FloatVect3 a_diff UNUSED, float body_v[GUIDANCE_INDI_HYBRID_V] UNUSED) {}

#endif
/**
*
* @param body_v
*
* WEAK function to set the quadplane wls settings
*/
#if GUIDANCE_INDI_HYBRID_USE_WLS
void WEAK guidance_indi_hybrid_set_wls_settings(float body_v[3] UNUSED, float roll_angle, float pitch_angle)
{
// Set lower limits
du_min_gih[0] = -guidance_indi_max_bank - roll_angle; // roll
du_min_gih[1] = RadOfDeg(guidance_indi_min_pitch) - pitch_angle; // pitch
du_min_gih[2] = (MAX_PPRZ - actuator_state_filt_vect[0]) * g1g2[3][0] + (MAX_PPRZ - actuator_state_filt_vect[1]) * g1g2[3][1] + (MAX_PPRZ - actuator_state_filt_vect[2]) * g1g2[3][2] + (MAX_PPRZ - actuator_state_filt_vect[3]) * g1g2[3][3];

// Set upper limits limits
du_max_gih[0] = guidance_indi_max_bank - roll_angle; //roll
du_max_gih[1] = RadOfDeg(GUIDANCE_INDI_MAX_PITCH) - pitch_angle; // pitch
du_max_gih[2] = -(actuator_state_filt_vect[0]*g1g2[3][0] + actuator_state_filt_vect[1]*g1g2[3][1] + actuator_state_filt_vect[2]*g1g2[3][2] + actuator_state_filt_vect[3]*g1g2[3][3]);

// Set prefered states
du_pref_gih[0] = -roll_angle; // prefered delta roll angle
du_pref_gih[1] = -pitch_angle; // prefered delta pitch angle
du_pref_gih[2] = du_max_gih[2];
}
#elif defined(GUIDANCE_INDI_QUADPLANE)
#warning We have GUIDANCE_INDI_QUADPLANE
void WEAK guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle)
{
float roll_limit_rad = GUIDANCE_H_MAX_BANK;
float max_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH);
float min_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MIN_PITCH);

float pitch_pref_rad = RadOfDeg(guidance_indi_pitch_pref_deg);

// Set lower limits
du_min_gih[0] = -roll_limit_rad - roll_angle; //roll
du_min_gih[1] = min_pitch_limit_rad - pitch_angle; // pitch
du_min_gih[2] = (MAX_PPRZ - stabilization_cmd[COMMAND_THRUST]) * guidance_indi_thrust_z_eff;
du_min_gih[3] = -stabilization_cmd[COMMAND_THRUST_X]*guidance_indi_thrust_x_eff;

// Set upper limits limits
du_max_gih[0] = roll_limit_rad - roll_angle; //roll
du_max_gih[1] = max_pitch_limit_rad - pitch_angle; // pitch
du_max_gih[2] = -stabilization_cmd[COMMAND_THRUST] * guidance_indi_thrust_z_eff;
du_max_gih[3] = (MAX_PPRZ - stabilization_cmd[COMMAND_THRUST_X])*guidance_indi_thrust_x_eff;

// Set prefered states
du_pref_gih[0] = -roll_angle; // prefered delta roll angle
du_pref_gih[1] = -pitch_angle + pitch_pref_rad;// prefered delta pitch angle
du_pref_gih[2] = du_max_gih[2]; // Low thrust better for efficiency
du_pref_gih[3] = body_v[0]; // solve the body acceleration
}
#else
void WEAK guidance_indi_hybrid_set_wls_settings(float body_v[3] UNUSED, float roll_angle UNUSED, float pitch_angle UNUSED) {

}
#endif

/**
* @brief Get the derivative of lift w.r.t. pitch.
*
Expand Down Expand Up @@ -845,21 +981,21 @@ int32_t guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv)
{
_gv = gv;
_v_mode = GUIDANCE_INDI_HYBRID_V_POS;
return (int32_t)thrust_in; // nothing to do
return (int32_t)stabilization_cmd[COMMAND_THRUST]; // nothing to do
}

int32_t guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv)
{
_gv = gv;
_v_mode = GUIDANCE_INDI_HYBRID_V_SPEED;
return (int32_t)thrust_in; // nothing to do
return (int32_t)stabilization_cmd[COMMAND_THRUST]; // nothing to do
}

int32_t guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv)
{
_gv = gv;
_v_mode = GUIDANCE_INDI_HYBRID_V_ACCEL;
return (int32_t)thrust_in; // nothing to do
return (int32_t)stabilization_cmd[COMMAND_THRUST]; // nothing to do
}

#endif
Expand Down

0 comments on commit 19875f6

Please sign in to comment.