Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 12:28:03 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

Sync guidance indi hybrid and guidance indi hybrid quadplane/tailsitter filters (#3541)

Browse Source
This commit is contained in:
NoahWe
2025-09-19 08:54:10 +02:00
committed by GitHub
parent 528f9a5ade
commit 6e0ee3a67a
4 changed files with 29 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
+3
View File
@@ -236,6 +236,7 @@ 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;
@@ -360,6 +361,7 @@ 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(&thrust_filt, tau, sample_time, 0.0);
init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);
@@ -404,6 +406,7 @@ 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(&thrust_filt, tau, sample_time, thrust_in);
init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
+4
View File
@@ -111,4 +111,8 @@ extern bool force_forward; ///< forward flight for hybrid nav
extern bool guidance_indi_airspeed_filtering;
extern bool coordinated_turn_use_accel;
extern Butterworth2LowPass roll_filt;
extern Butterworth2LowPass pitch_filt;
extern Butterworth2LowPass yaw_filt;
#endif /* GUIDANCE_INDI_HYBRID_H */
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_quadplane.c
+14 -12
View File
@@ -44,7 +44,13 @@
float guidance_indi_thrust_z_eff = GUIDANCE_INDI_THRUST_Z_EFF;
#endif
float bodyz_filter_cutoff = 0.2;
#ifndef GUIDANCE_INDI_BODYZ_FILTER_CUTOFF
#ifdef GUIDANCE_INDI_FILTER_CUTOFF
#define GUIDANCE_INDI_BODYZ_FILTER_CUTOFF GUIDANCE_INDI_FILTER_CUTOFF
#else
#define GUIDANCE_INDI_BODYZ_FILTER_CUTOFF 3.0
#endif
#endif
Butterworth2LowPass accel_bodyz_filt;
@@ -53,7 +59,7 @@ Butterworth2LowPass accel_bodyz_filt;
* Call upon entering indi guidance
*/
void guidance_indi_quadplane_init(void) {
float tau_bodyz = 1.0/(2.0*M_PI*bodyz_filter_cutoff);
float tau_bodyz = 1.0/(2.0*M_PI*GUIDANCE_INDI_BODYZ_FILTER_CUTOFF);
float sample_time = 1.0 / PERIODIC_FREQUENCY;
init_butterworth_2_low_pass(&accel_bodyz_filt, tau_bodyz, sample_time, -9.81);
}
@@ -78,17 +84,13 @@ void guidance_indi_quadplane_propagate_filters(void) {
* @param body_v 3D vector to write the control objective v
*/
void 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]) {
// Get attitude
struct FloatEulers eulers_zxy;
float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
/*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);
float sphi = sinf(roll_filt.o[0]);
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]);
#ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
#define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid_tailsitter.c
+8 -12
View File
@@ -39,18 +39,14 @@
*/
void 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]) {
// Get attitude
struct FloatEulers eulers_zxy;
float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
/*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);
float sphi = sinf(roll_filt.o[0]);
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]);
//minus gravity is a guesstimate of the thrust force, thrust measurement would be better
#ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
@@ -58,13 +54,13 @@ void guidance_indi_calcg_wing(float Gmat[GUIDANCE_INDI_HYBRID_V][GUIDANCE_INDI_H
#endif
/*Amount of lift produced by the wing*/
float pitch_lift = eulers_zxy.theta;
float pitch_lift = pitch_filt.o[0];
Bound(pitch_lift,-M_PI_2,0);
float lift = sinf(pitch_lift)*9.81;
float T = cosf(pitch_lift)*-9.81;
// get the derivative of the lift wrt to theta
float liftd = guidance_indi_get_liftd(stateGetAirspeed_f(), eulers_zxy.theta);
float liftd = guidance_indi_get_liftd(stateGetAirspeed_f(), pitch_filt.o[0]);
Gmat[0][0] = cphi*ctheta*spsi*T + cphi*spsi*lift;
Gmat[1][0] = -cphi*ctheta*cpsi*T - cphi*cpsi*lift;