Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-24 05:45:59 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

filt yaw difference and ZXY order for quadplane guidance (#3624)

Browse Source 
* filt yaw difference and ZXY order for quadplane guidance

* small fix

* fix yaw filt guidance quadplane
This commit is contained in:
NoahWe
2026-03-31 15:48:11 +02:00
committed by GitHub
parent 68720b360d
commit 846989b2d2
4 changed files with 33 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
+19 -4
View File
@@ -236,10 +236,12 @@ float thrust_act = 0.f;
Butterworth2LowPass filt_accel_ned[3];
Butterworth2LowPass roll_filt;
Butterworth2LowPass pitch_filt;
Butterworth2LowPass yaw_filt;
Butterworth2LowPass thrust_filt;
Butterworth2LowPass accely_filt;
Butterworth2LowPass guidance_indi_airspeed_filt;
static Butterworth2LowPass yaw_delta_filt;
static float previous_yaw_raw = 0.0f;
float yaw_filt = 0.0f;
struct FloatVect2 desired_airspeed;
float gi_unbounded_airspeed_sp = 0.f;
@@ -361,7 +363,9 @@ void guidance_indi_init(void)
}
init_butterworth_2_low_pass(&roll_filt, tau, sample_time, 0.0);
init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, 0.0);
init_butterworth_2_low_pass(&yaw_filt, tau, sample_time, 0.0);
init_butterworth_2_low_pass(&yaw_delta_filt, tau, sample_time, 0.0);
previous_yaw_raw = 0.0f;
yaw_filt = 0.0f;
init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, 0.0);
init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);
@@ -406,7 +410,10 @@ void guidance_indi_enter(void)
init_butterworth_2_low_pass(&roll_filt, tau, sample_time, eulers_zxy.phi);
init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, eulers_zxy.theta);
init_butterworth_2_low_pass(&yaw_filt, tau, sample_time, eulers_zxy.psi);
init_butterworth_2_low_pass(&yaw_delta_filt, tau, sample_time, 0.0);
// Initialize yaw tracking variables
previous_yaw_raw = eulers_zxy.psi;
yaw_filt = eulers_zxy.psi;
init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, thrust_in);
init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);
@@ -879,7 +886,15 @@ void guidance_indi_propagate_filters(void)
update_butterworth_2_low_pass(&roll_filt, eulers_zxy.phi);
update_butterworth_2_low_pass(&pitch_filt, eulers_zxy.theta);
update_butterworth_2_low_pass(&yaw_filt, eulers_zxy.psi);
// Filter yaw delta instead of raw angle to handle wrapping properly
float yaw_delta = eulers_zxy.psi - previous_yaw_raw;
FLOAT_ANGLE_NORMALIZE(yaw_delta); // Normalize to [-pi, pi]
update_butterworth_2_low_pass(&yaw_delta_filt, yaw_delta);
previous_yaw_raw = eulers_zxy.psi;
// Accumulate filtered delta
yaw_filt += yaw_delta_filt.o[0];
FLOAT_ANGLE_NORMALIZE(yaw_filt);
// Propagate filter for sideslip correction
float accely = ACCEL_FLOAT_OF_BFP(stateGetAccelBody_i()->y);
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
+1 -1
View File
@@ -112,6 +112,6 @@ extern bool coordinated_turn_use_accel;
extern Butterworth2LowPass roll_filt;
extern Butterworth2LowPass pitch_filt;
extern Butterworth2LowPass yaw_filt;
extern float yaw_filt;
#endif /* GUIDANCE_INDI_HYBRID_H */
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c
+11 -10
View File
@@ -89,8 +89,8 @@ void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_H
float cphi = cosf(roll_filt.o[0]);
float stheta = sinf(pitch_filt.o[0]);
float ctheta = cosf(pitch_filt.o[0]);
float spsi = sinf(yaw_filt.o[0]);
float cpsi = cosf(yaw_filt.o[0]);
float spsi = sinf(yaw_filt);
float cpsi = cosf(yaw_filt);
#ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
#define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0
@@ -102,21 +102,22 @@ void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_H
// 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;
// ZXY Euler order
Gmat[0][0] = 0.0f;
Gmat[1][0] = -cphi*ctheta*lift_thrust_bz;
Gmat[2][0] = -ctheta*sphi*lift_thrust_bz;
Gmat[0][1] = cphi*ctheta*lift_thrust_bz*GUIDANCE_INDI_PITCH_EFF_SCALING;
Gmat[0][1] = 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[0][2] = stheta;
Gmat[1][2] = -sphi*ctheta;
Gmat[2][2] = cphi*ctheta;
Gmat[0][3] = ctheta;
Gmat[1][3] = 0;
Gmat[2][3] = -stheta;
Gmat[1][3] = sphi*stheta;
Gmat[2][3] = -cphi*stheta;
// Make this term zero to prevent switching 'exploits'
// Gmat[2][3] = 0;
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitter.c
+2 -4
View File
@@ -45,8 +45,8 @@ void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_H
float cphi = cosf(roll_filt.o[0]);
float stheta = sinf(pitch_filt.o[0]);
float ctheta = cosf(pitch_filt.o[0]);
float spsi = sinf(yaw_filt.o[0]);
float cpsi = cosf(yaw_filt.o[0]);
float spsi = sinf(yaw_filt);
float cpsi = cosf(yaw_filt);
//minus gravity is a guesstimate of the thrust force, thrust measurement would be better
#ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
@@ -102,5 +102,3 @@ void guidance_indi_hybrid_set_wls_settings(float body_v[3] UNUSED, float roll_an
#endif